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ABSTRACT 



The Naval Postgraduate School in Monterey, California is 
currently working on an ongoing project for research in 
autonomous underwater vehicle (AUV) technology. This pro- 
ject comprises two areas of research. The first area is 
research conducted on the system, NPS II AUV. The second 
area is a computer simulation of the actual system. One 
topic which is vital to both areas is three-dimensional path 
planning. The concept of three-dimensional path planning is 
on the order of magnitude of polynomial time and current 
research in this area is limited. This paper reviews my 
findings and submits an algorithm which finds a best path in 
a three-dimensional environment, while avoiding all known 
polyhedral obstacles. The algorithm's concept is to reduce 
the three-dimensional world to a series of two-dimensional 
representations, allowing the algorithm to use tangential 
lines created from the start to nodes on the polygons lying 
between the start and goal, from nodes on polygons to other 
polygon nodes and finally, from polygon nodes to the goal. 
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I . INTRODUCTION 



A . BACKGROUND 

The last few years, technology has advanced 
significantly to see the use of unmanned vehicles 
substantially increase for land, sea and air applications. 
With regards to military applications, tacticians have used 
this technology to improve and extend the range and 
effectiveness of sensors and weapons. In the recent United 
States, and allied coalition forces' invasion of Iraq in 
DESERT STORM, both land and sea forces used remotely 
operated planes to perform reconnaissance of enemy forces 
with remarkable success . There are other examples of the 
successful use of remotely operated vehicles. For example, 
the Israeli Air Force successfully used remotely operated 
vehicles to perform reconnaissance and intelligence 
collection of enemy strong points prior to air campaigns 
conducted in 1982 , resulting in zero losses for the Israelis 
and substantial damage inflicted on enemy forces. (Floyd 
91 ) . 

Besides air operations, the concept of remotely operated 
vehicles can be incorporated in naval operations . The 
United States Navy's current maritime strategy calls for its 
forces to be forward deployed as possible. This results in 
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US Naval forces operating in or near enemy controlled 
waters. However, since Soviet strategies for both land and 
sea operations call for military operations to be planned 
for and conducted in an in-depth multi-layered unit 
configuration for both offensive and defensive operations, 
the use of a remotely operated tethered vehicle with a 
nearby mother ship acting as the brain is not feasible. 
Therefore, with the US Navy's current strategy coupled with 
the Soviet Union's strategy of multi-layered unit 
configurations, the use of autonomous underwater vehicles 
(AUV) for potential military operations could significantly 
improve the US Navy's operational capabilities. These AUVs 
must possess sufficient onboard sensors as well as control 
units and be able to perform a preplanned mission without 
external control. (Robinson 86) 

Even with the current decline of the Soviet Union as a 
world leader due to its internal conflicts and the decline 
of its satellite countries, the use of AUV technology is 
still a viable option. This became apparent with the events 
and operations of DESERT SHIELD and DESERT STORM in the 
Persian Gulf, especially those dealing with mining of the 
gulf by the Iraqi naval forces. In addition to using an AUV 
in mine warfare, an AUV can also be used in many different 
military operations. Examples of some basic types of 
operations follow: 
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1. reconnaissance (both covert and overt) for 
intelligence gathering 

2. surveillance 

3. mine warfare 

4 . terrain mapping 

5. supply and resupply for covert units along the coast 
munitions delivery (or an intelligent torpedo) 

The use of an AUV for these type of operations would 
significantly reduce equipment and personnel to enemy 
capabilities, thus reducing operational costs no matter the 
cost of the AUV. Advances in computer technology, 
particularly in Artificial Intelligence, have made it 
possible to begin AUV development for these type of 
missions . 

However, concept of traveling from A to B without human 
interaction requires that the system act and think as a 
human would. One particular area which offers significant 
challenges is path planning such that the system avoids all 
known obstacles within the environment. In a three- 
dimensional environment, the challenge is even more 
interesting since there exists an infinite amount of 
possible paths extending from point A to point B. This 
concept of three-dimensional path planning is also not 
limited to an AUV system, but can be used on any system 
which must traverse a three-dimensional environment. 
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B . AUV CONCEPT 



The basic design of an AUV calls for an unmanned 
submersible vehicle with onboard systems and sub-systems 
that provide power, motion control, navigation, obstacle 
detection and collision avoidance with the capability to 
perform a preplanned mission by controlling and monitoring 
onboard systems without any external input. In addition, 
the AUV must be able to replan its mission in the event it 
encounters unplanned for events . This includes onboard path 
replanning to the mission goal in the event the AUV 
encounters an unplanned-f or obstacle (Floyd 91). The Naval 
Postgraduate School (NPS) AUV II models this basic concept 
of AUV development; however, an AUV capable of performing 
the type of missions listed above will need to carry 
additional equipment tailored to perform the specific 
mission in order to accomplish the mission. 

C. PATH PLANNING AND REPLANNING 

The concept behind path planning is to find the shortest 
path or a relatively short path from an initial starting 
point to a goal, avoiding all known obstacles in the 
environment. This concept results in a preplanned mission 
which is then stored onboard on of the memory modules of the 
AUV. This path planning process can be accomplished using 
the AUV control unit, but since this process is a-priori to 
the mission execution, it would be more feasible to do the 
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process on another system with more capability, This allows 
for faster results determining a path, multiple path 
planning missions and leaves the AUV for other mission 
executions. However, the replanning concept calls for the 
path planning process to be performed on the AUV. The 
concept of an onboard mission planner further implies that 
the planner must have the capability to access onboard 
stored information of the environmental model. It further 
implies that the replanning process must be in strict 
coordination with not only the obstacle avoidance capability 
on the AUV, but also the module for location determination. 
This thesis will address the path planning and replanning 
process. Work on the obstacle avoidance and associated 
problems has been addressed by another NPS AUV II project 
team member (Floyd 91). 

D. OBJECTIVES 

This thesis will address the following research 
questions : 

1. How to define the typical underwater environment? 

2. Given a known underwater world environment, how to plan 
a route which is the shortest path or a relatively 
short path and avoids all known polyhedrons in the 
environment? 

3. How to incorporate the path planner as a on-board 
replanner? 
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E. THESIS ORGANIZATION 



The organization of this thesis is in five chapters. 
Chapter II contains background information concerning the 
NPS AUV II system along with a general summary of other AUV 
projects ongoing as well as research conducted on path 
planning. Chapters III and IV discuss implementation of the 
path planning process . 

The main idea behind any path planning algorithm is two 
fold. First, the planner must determine if there exists any 
obstacles lying between the starting point and the ending or 
goal point. If there are not any obstacles, then the path 
from start to goal is the straight line path from start to 
goal. However, if there is one or more obstacles between 
the start and goal, then the path planner must begin the 
planning process to determine the shortest of a relatively 
short path. 

The organization of this thesis follows this idea. 
Chapter III begins with a discussion of how the three- 
dimensional model is depicted. Afterwards, the rest of the 
chapter is devoted to describing the technique used to 
determine if one or more obstacles lie between the start and 
goal. Chapter IV describes in detail the path planning 
process used to determine a best path from start to goal if 
there is one or more obstacle between them. Finally, 

Chapter V are my conclusions and recommendations for future 
research . 
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II. NPS AUV PROJECT AND RELATED WORK 



A. SURVEY OF PREVIOUS WORKS 

As stated before, the primary goal of any AUV research 
is to allow the vehicle to perform a mission without human 
intervention during the execution of the mission. To 
operate autonomously, the vehicle must possess adequate 
intelligence to travel a pre-planned route, but also react 
to unplanned for situations. There are several ongoing AUV 
research projects as well as research for path planning for 
both two-dimensional and three dimensional. 

1. Autonomous Underwater Vehicle Projects 

Within the past decade, research on Autonomous 
Vehicle control has made great strides. There are several 
ongoing projects for AUV development conducted by not only 
academic institutions, but also government agencies as well. 

Texas A&M University is one of the academic 
institutions working on AUV research. Most project designs 
not only incorporate the software or an on-board computer 
controller, but also the specific hardware that the 
controller controls. Texas A&M University has taken a 
different approach in the project development. Their 
approach is to develop a generic computer controller capable 
of successfully performing a fully autonomous long range 
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underwater mission. Table I shows the mission requirements. 
While not specifically building the hardware for the 
controller to control, a generic vehicle was considered in 
order to provide design parameters in developing the 
Table I - TEXAS A&M MISSION REQUIREMENTS 

MISSION REQUIREMENTS 

Long Range Capability - 3000 Nautical Miles 
Long Duration - 300 Hours 
Precise Navigation Capability 
Collision Avoidance 

Communication Capability with Mother Ship 
High Reliability and Adaptability 

controller. As can be seen from Table II, the vehicle 
requirements are unrealistic for any type of testing other 



Table II - TEXAS A&M VEHICLE CHARACTERISTICS 



VEHICLE 


CHARACTERISTICS 


Length 


81 feet 


Diameter 


13 feet 


Top Speed 


12 knots 


Cruise 


10 knots 


Maximum Depth 


800 feet 


Range 


3000 Nautical Miles 
3 00 hours (3 10 NM/Hour 


Fuel Load 


8.1 tons Diesel 


Computational Power 


16 SUN Sparc Stations 
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than simulation testing. Texas A&M researchers use a large 
vehicle in the development phase with the idea that the 
generic controller can be adapted for other vehicle designs; 
however, with the use of such a large computer platform in 
the design, one major question is whether the controller can 
be adapted to a platform which does not have 16 SUN Sparc 
Stations for computational power. (TEXAS A&M 90) 

The United States Navy in conjunction with DARPA 
initiated an AUV program in 1988 with the primary purpose of 
showing that AUV systems could successfully perform certain 
Navy mission requirements. Table III shows these mission 
requirements along with some of the vehicle characteristics. 
Unlike the Texas A&M project, DARPA is developing a complete 
system of both hardware and software along with a real-time 
simulation capability of the AUV. And like the Texas A&M 
project, DARPA' s AUV is a long range platform and not 
suitable for operations within a small area such as a harbor 
facility. (Pappas et.al. 91) 

Table III - DARPA MISSION AND VEHICLE CHARACTERISTICS 

MISSION AND VEHICLE CHARACTERISTICS 

1. Mission 

a. Tactical Acoustic System 

b. Mine Search System 

c. Remote Surveillance System 

2. Vehicle Characteristics 



a . 


Weight 


7.5 tons 


b . 


Depth 


1500 feet 


c . 


Speed 


10 knots 


d. 


Range 


240 Nautical Miles 




24 hours (3 10 knots 
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The University of New Hampshire has done extensive 
research on AUV technology. There initial project began in 
1978 with the completion of the first Experimental 
Autonomous Vehicle (EAVE) and subsequent successful test of 
autonomously following an underwater pipeline. In 1983 this 
vehicle successfully followed a pre-defined path using 
acoustic transponder navigation. In 1986, the university 
built two new vehicles similar to the original, but with 
more computational power. Current interest in their 
research is to use artificial intelligence techniques to 
develop a knowledge based guidance and control system. 
(Blidberg 90) 

The Institute of Industrial Science at the 
University of Tokyo has developed their own version of an 
AUV. The PTEROA150 was built and successfully tested in 
1989. After successfully testing this prototype, the 
university built the PTEROA250 with greater capabilities as 
well as using neural-net technology for control of the 
vehicle. The Institutes research has shown that a neural 
net controller successfully controls the AUV and can be 
adjusted to allow the vehicle to be used in different 
environmental conditions. (Ura 90) 

2 . Path Planning 

Articles which present the more important aspects of 
path planning are presented. To date, there has been a 
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considerable amount of research on path planning in a two- 
dimensional model. However, there is limited amounted of 
research for a three-dimensional model. These articles can 
be classified into two main categories of path planning. 
These categories are graph searching techniques and 
potential field methods. In addition, the majority of 
articles that do deal with a three-dimensional model find 
only paths based on the shortest path and do not consider or 
simplify other constraints such as time allotted for travel, 
energy consumption, type of mission, and type of 
environment . 

M. Sharir has done extended research in the area of 
path planning and obstacle avoidance. Sharir presents 
several algorithms for both two-dimensional and three- 
dimensional environments (Sharir 87). He and J. T. Schwartz 
have also collected a group of algorithms on motion planning 
(Schwartz 88). The algorithms they present are 
predominantly graph searching techniques and their work 
catalogs the algorithms into different classes based on each 
of the algorithms properties. Their work gives a good basis 
for further research; however, as they point out, they 
present no research on algorithms based on a best cost 
property other than shortest euclidean distance since there 
is very little research ongoing. 

In his thesis, Ong describes a heuristic search 
algorithm that finds a viable path in a three-dimensional 
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grid coordinate system. Ong uses a graph searching technique 
and the algorithm, as the name implies, uses detailed 
heuristic to prune the solution space by selecting only 
viable successor states for further search. The heuristic 
that are used allow the vehicle to model the human decision- 
making process in determining which viable path is followed. 
Since it uses heuristic extensively, it can be considered as 
an informed search and gives a semi-optimized solution. It 
uses a simplified energy consumption rate of 1.2 times the 
overall length of the path for a path with the start and 
goal lying at different depths. For a path with turns, the 
algorithm adds a constant for the size of the turning angle. 
As the size of the turning angle increases, the larger this 
cost constant becomes . This is a better estimate than the 
estimate than the estimate used for depth change; however, 
it is not accurate. (Ong 89) 

Charles Warren uses the potential fields method to 
determine a viable path. In his research, Warren uses 
artificial potential fields in a global path planning vice a 
locally selected region. The idea behind his research is to 
apply potential fields around configuration space (C-space) 
obstacles and use these fields to select a safe path. The 
first step in the process is to map the environment into the 
C-space of the robot. Then artificial fields are placed 
around the obstacles in the environment. In a sea 
environment, many obstacles begin at the ground and extend 
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up. Since the sea floor is an obstacle as well as these 
protrusions, the result is one massive obstacle. Warren 
compensates by creating the potential fields from horizontal 
planes sliced through the obstacles. Each of these planes 
forms a forbidden region similar to the two-dimensional 
problem. Since Warren's method uses a global method, a 
chosen path is selected and then modified using the 
potential fields to influence its direction. An advantage 
to using this technique is that the probability of becoming 
trapped by a local minimum is greatly reduced since the path 
planning process uses a known viable path. Warren's 
research shows a viable and fast path planning process. 
However, it has its limitations. A major limitation is that 
the algorithm must know the global environment prior to the 
process beginning. (Warren 89 and Warren 90) 

Martin Herman introduces a fast three-dimensional, 
collision-free motion planner in his research at the 
National Bureau of Standards. In his planner, Herman 
represents the environment in an Octree structure. Using an 
Octree structure, the environment is initially represented 
as a single primitive geometric structure, usually a cube. 
This cube is the root of a octree. Children of a node are 
created if the parent cubic volume is not homogeneous or 
completely filled with an object or completely empty. If 
the cubic volume is not homogeneous, then the cube is 
subdivided into eight cubes (called octants) with each cube 
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becoming a child of the parent. Once the environment is 
represented in an Octree structure, Herman uses one of three 
search algorithms. They are A*, hill climbing, and 
hypothesize and test. The path found using one of these 
algorithms finds a collision-free path, but it is not 
necessarily the shortest path. As Herman states, finding 
the shortest path is computational expensive and a 
relatively short path is adequate for many tasks. One other 
major limitation to this method of path planning is that 
considerable memory is required to store the octree. 

(Herman 89) 

Perez and Wesley have introduced in their research, 
an algorithm that works for both two-dimensional and three- 
dimensional models. In determining a safe shortest path for 
an autonomous vehicle, the authors' algorithm grows each 
obstacle by some parameter that corresponds to the 
dimensions of the vehicle. With the extra dimensions added 
to each obstacle, the best path is assured to be a safe 
path. The algorithm uses a parametric function to allow for 
vehicles that are not able to change their posture in the 
environment. The algorithm uses the vertices, represented 
in the x, y coordinate system, of the obstacle in the two- 
dimensional environment in finding a shortest path. In the 
three-dimensional environment, the grown obstacles are 
represented in the x, y, z coordinate system. However, a 
shortest path whose node set contains only vertices of these 
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grown obstacles will not necessarily be the shortest path. 

As a result, the authors use additional vertices of a 
constant distance from each other. The resulting path is a 
good approximation of the optimal path. (Perez et.al. 79) 

Kanayama has introduced work on two-dimensional path 
planning for his ongoing research with the Yamabico Mobile 
Robot. In his algorithm, Kanayama proposes that fast 
effective path planning can be accomplished using tangent 
lines. In using technique, possible paths are determined by 
determining the tangent lines between the start point and 
all possible obstacles, between the possible obstacles and 
other obstacles and finally the obstacles and the goal point 
(Kanayama 90). 

There has been a significant amount of research 
conducted in pruning or reducing the search space of the 
visibility graph. Montgomery has introduced several 
techniques that does such. One of more interest is the 
technique of finding the farthest points to the left and 
right with relation to the point being considered for 
expansion. Once these points are found, Montgomery's 
algorithm determines the front edge of the obstacle and 
strips off all vertices that comprise the back edge. He 
argues that by stripping off the back edges, the 
computational requirements are greatly reduced since the 
number of vertices is reduced by roughly a factor of two. 

The algorithm also strips from the search space any 
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obstacles that are occluded from the expansion point by 
another obstacle. (Montgomery et.al. 87) 

As can be seen, there is a considerable amount of 
research on path planning in a three-dimensional 
environment. Each of the above methods are viable and 
efficient as a motion planner. However, these methods only 
use a shortest path (or a relatively shortest path) to 
determine which path is the best path to use. I found no 
research which addressed using some other measure to 
determine a path other than shortest path. 

B. THE NPS AUV PROJECT 

The NPS AUV PROJECT is currently comprised of two 
different areas or research. The first area research is 
computer simulation of the overall project in the 
laboratories at NPS; the second area is experimental 
research conducted on the NPS AUV II, using the NPS swimming 
pool as the world environment. 

1. The NPS AUV II Vehicle 

The current NPS AUV vehicle is called the NPS AUV 
II. Its design is based on development of the earlier and 
smaller prototype, the NPS AUV I (Healey et.al. 89). The 
basic vehicle design of the NPS AUV II has been detailed by 
Good (Good 89). Figure 1 illustrates the basic design of 
the vehicle. 
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Figure 1 - NPS II AUV 



The NPS AUV II is a submersible vehicle capable of a 
maximum speed of two knots. It has an overall length of 93 
inches . The main body of the vehicle is made of aluminum 
and the constructed as a box. It has a beam of 16 inches, a 
height of 10 inches and a length of 72 inches. The nose 
cone is constructed of fiberglass and is 21 inches long, 
extending the length of the vehicle to 93 inches. The 
vehicle displaces 380 pounds and uses fixed ballast. The 
vehicle has four forward control surfaces, four aft control 
surfaces, four tunnel thrusters and two aft screws each 
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powered by 24 volt DC drive motors, providing 1/8 horsepow- 
er. With the eight control surfaces, the vehicle is highly 
maneuverable and has a turning radius of 3 ship lengths . 

The vehicle's power sources are lead-acid gel batteries 
capable of providing up to two hours of propulsion and 
computational power. 

On board sensors include the following systems: 

1. a navigation system sensor comprised of a flux gate 
compass and directional gyroscope, a vertical gyroscope 
and a three axis rate gyroscope system with transla- 
tional accelerometers . 

2. a paddle-wheel speed sensor installed in the nose. 

3. four sonar transducers installed in the nose. 

The on-board computational power is supplied by a GESPAC 
MPU 30HZ processor with a Motorola 68030 CPU. In addition, 
the system also has two megabyte of RAM and a 68882 math 
coprocessor running at 25 MHZ. The operating system is the 
OS-9 multi-tasking operating system. 

2 . Computer Simulation 

The computer simulation portion of the NPS AUV 
PROJECT is currently done on a high resolution graphics 
works -station . The basic simulation model of the NPS AUV II 
was developed by Jurewicz (Jurewicz 91). This model uses 
up-to-date performance coefficients characteristic of the 
NPS AUV II. The basic concept of the simulator is to allow 
project team members to integrate individual modules of the 
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project into the simulator for testing prior to 
implementation on-board the NPS AUV II. The following 
modules have been implemented on the simulator: 



1. A simple locomotion generator for generating a path 
given a set of reference points along a path. This 
generator is restricted to a path in which the 
reference points of the inputted set lying on the same 
horizontal plane. 

2. A three-dimensional guidance control module (Magrino 
91) 

3. An obstacle detection and avoidance module using sonar 
technology. This module has also been successfully 
implemented on the NPS AUV II. (Floyd 91) 
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III. OBSTACLE REPRESENTATION 



One of the first problems I had to solve was determining 
how to represent each three-dimensional obstacle in the 
environmental model . There were two areas I had to 
consider in deciding how to do the representation. 

The first area was that I wanted my path planner to use 
a tangential method similar to the way Kanayama and Crane 
approach two-dimensional path planning (Kanayama 90 and 
Crane 91). The other area of concern was that I wanted the 
path planner to be compatible with not only the NPS pool 
environment where actual testing of the NPS II AUV is 
conducted but also the Monterey Bay, which will be included 
in the simulator at a later date. 

In addition to considering these two concerns, I also 
made some assumptions of the type of obstacles and the type 
of environment. One such assumption I made concerning the 
obstacles was that all polyhedrons were considered to be 
convex in nature. The assumption I made about the 
environment was that if a path existed between the start and 
the goal, it was not a path that first retreated in the 
opposite direction. A path which first retreats in the 
opposite direction is any path that begins in a direction in 
the x-y plane which is plus or minus 180 degrees different 
from the vector from start to goal in the x-y plane. 
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The bay data was collected in two different resolutions. 
The first resolution covered a large area and used a 
distance between depth points of 200 meters; the second 
resolution, while much smaller gave accuracy with a distance 
of 30 meters between depth points. With the location and 
depth known for each point, an XYZ coordinate system could 
be used in computations. In addition, as with the bay data, 
an XYZ coordinate system could be used for the NPS Pool and 
its environment. 

With these two concerns in mind, I decided to represent 
each obstacle in a triply linked list with each obstacle 
composing the main link, each obstacle-face composing the 
obstacle link and each vertex of the face composing the 
obstacle-face link. In addition, the obstacle list 
comprises first the side faces, followed by the top face if 
needed, and then the bottom face if needed. Also, each 
obstacle face list is a doubly linked list and the vertices 
of the face are placed in a counter-clockwise order. Figure 
2 shows the basic concept in this link list structure. 

The size of the link list can become quite large 
especially for a real world environment such as the Monterey 
Bay. However, the size of this link list can be pruned once 
the user inputs the start point and the goal point. For 
example, once the user inputs these points, the algorithm 
can prune from the list all obstacles which lie behind the 
start goal. 
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IV . PATH PLANNING 



If the visibility check between the start and goal 
points determines that there are obstacles lying between the 
two points, the path planning process begins. With path 
planning in a two-dimensional environment, the process is 
relatively simple since all paths are on the same plane or 
surface. However, three-dimensional path planning does not 
have the same characteristics . The three-dimensional 
environment has an infinite amount of planes and possible 
paths to consider. As a result, I had to develop a process 
which reduced the search space and the number of possible 
paths to consider to a reasonable amount. In developing the 
algorithm, I considered only obstacles which rise from the 
floor of the environment and are convex. By considering 
only these type of obstacles, I do not have to consider a 
possible path which goes underneath an obstacle. Even 
though, as Sharir states, the problem still approaches 
polynomial time in order of magnitude (Sharir 87). 

A. BASIC CONCEPTS OF THREE DIMENSIONAL PATH PLANNING 

In developing an algorithm to find a best path in a 
three dimensional environment, I decided to extend 
Kanayama's two-dimensional path planning algorithm. 
Kanayama's technique develops tangents from the expansion 
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point to each valid two-dimensional polygon in determining a 
best path (Kanayama 90). In addition to using expanding on 
his technique, I will also expand on Kanayama' s terminology. 

1. Polyhedron Intersection 

In order to solve the problem of determining whether 
or not a polyhedron lies between an expansion point and the 
goal, I used vector calculus and developed a simply ray 
tracing algorithm to determine visibility. A point is 
visible with another point if and only if no polyhedrons 
intersect the vector formed by the two points . This 
process of determining if two points are visible or not is 
not only used at the beginning of the path planning process 
with the start and goal points used, but also each time the 
path planning process expands a path. Therefore, for 
explanatory reasons, I use the term "Expanding Point" and 
its variations in the explanation to denote the first point 
used. Similarly, I use "End Point" to denote the point the 
process is expanding to. 

a. General Description. 

The process initially finds the ray formed by the 
expansion point and the goal. From the ray, the process 
determines the vector. Once this vector is found, the 
process finds the equation of the plane formed by the first 
face of the first polyhedron in the list of polyhedrons, 
determines if there exists an intersection between the 
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expansion-goal ray and the plane, and if one exists, 
determines if the intersection point lies within the 
boundaries of the polyhedron face. If the intersection 
point lies between the boundaries, the process ends 
notifying the calling process that a polyhedron lies between 
the two points. Otherwise, the process continues checking 
each face of each polyhedron, notifying the calling process 
that the two points are visible to each other. Table IV 
depicts the pseudo language I developed for the process . 



Table IV - PSEUDO LANGUAGE FOR INTERSECT POLYHEDRON 



intersect p olyhedron (ptl, pt2, 3D_world) 

{ 

for (each polyhedron in 3D_world){ 
for(each face on polyhedron) { 
plane = f ind_plane_equation ( 3 points on face); 
intersection_pt = f ind_intersection_point (ptl, pt2 , 

plane ) ; 

if ( intersection_pt != NO_INTERSECTION) { 

if ( lines_intersection ( intersection , face, plane) 
return (INTERSECT) ; 

} 

} 

} 

return (NO_INTERSECT) : 



b. Directed Ray. 

The process begins by determining the parametric 
equations for the ray formed using the expansion point and 
the goal. I directed the ray from the expansion point and 
the goal so the equations take on the form in Equation 1. 



25 



Eq 1 



X = x e + (x g - x e ) t 

y = y e + (y g - y e ) t 

z = z e + (z g - z e ) t 
However, in this case, the direction of the ray does not 
matter and adopted this manner for consistency and 
readability . 

c. Plane Equation. 

In determining the equation of a plane formed by 
a polyhedron face, the process uses 3 points [ z 1 ) , 

( x 2 /Y 2 ' z 2 )' ( x 3 'Y 3 , z 3 )} from the polyhedron face the process 

is checking. The process uses these 3 points to determine 2 
vectors in the form of Equations 2 and 3 . 

V 2 = (x 2 - x 3 ) I + (y 2 - y 3 ) J + (z 2 - z x )JC Eq 2 

Vj = (x 3 - x x ) I + (y 3 - yjj + (z 3 - z 3 )JC Eq 3 

Once these two vectors are found, the process finds the 
normal vector to the plane by determining the dot product of 
the two vectors as seen in Equation 4 . 

N - V 1 ' V 2 Eq 4 

N also takes on the form 

N = n^I + n 2 J + n 2 K Eq 5 

Using Equation 5, the equation of the plane can be found by 

substituting a point on the plane into the equation. If the 

point (x 1 ,y 1 ,z 1 ) is used, the result is Equation 6. 

- x 3 ) + n 2 {y - y 3 ) + n 3 {z - z x ) =0 Eq 6 

Equation 6 equates to Equation 7 

nix + n 2 y + n 3 z - D - 0 Eq 7 
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where D equates to Equation 8 . 

D = + n 2 y 2 + n z z 2 Eq 8 

The source code I developed to find the plane equation is 
shown in Table V and in Appendix C. 



Table V - FIND PLANE EQUATION SOURCE CODE 



equation 

point 

{ 

point 

equation 



find_plane_equation(ptl, pt2, pt3 ) 
ptl, pt2 , pt3 ; 

vector 1_2 , vector 1_3 ; 
pl_eq; 



vector l_2.x = 
vectorl_2.y = 
vector 1_2 . z = 
vector l_3.x = 
vector l_3.y = 
vectorl 3 . z = 



pt2.x - ptl.x; 
pt2.y - ptl.y; 
pt2.z - ptl.z; 
pt3.x - ptl.x; 
pt3.y - ptl.y; 
pt3.z - ptl.z; 



/★cross product is the standard cross product 
equation. Function also is in 3dd.c*/ 
pl_eq = cross_product (vector 1_2 , vector 1_3 ) ; 
pl_eq.d = ((pl_eq.x * (-1.0) * ptl.x) + 

(pl_eq.y * (-1.0) * ptl.y)); 
pl_eq.d = pl_eq.d + (pl_eq.z * (-1.0) * ptl.z); 
return (pl_eq); 



d. Intersection Point. 

Since the process needs to find the intersection 
point formed by the ray and the plane, the values for x, y, 
and z used in Equation 1 can be substituted for x, y, and z 
in Equation 7. This results in Equation 9. 
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Eq 9 



n 1 {x e + {x g - x e ) t - x x ) + 
n 2 (y e + (y* - y e ) t - y x ) + 
n 3 (z e + (z g - z e ) t - z x ) = 0 



Finally, Equation 10 results when t is solved. 



t = — 



n i x e + n 2 y 0 + ^ 3 y e + D 



Eq 10 



n x ( x g + x e ) + n 2 (y ff + y Q ) + n 3 (z ff + z E ) 

If we let w and Y take the form of Equations 11 
and 12, then t equates to Equation 13 

w = n x x a + n 2 x e + n 3 y e + D Eq 11 

Eq 12 
Eq 13 



y = n x ( x g - x e ) + n 2 ( y g - y e ) + n 3 (z g - z ff ) 

y 



t = 



In equating t to Equation 13, there is a check in 
the value of y. By first computing y and determining if 
y = 0, then the ray is parallel to the plane, no other 
computations are necessary, and the function returns to the 
calling function that there is no intersection for the 
checked plane face. If y != 0, then the process finds the 
value for the parameter w. 

If 0 < t < 1, then the ray intersects the plane 
between the two points. If t < 0 or t > 1, then the ray 
intersects the plane, but not on the line segment formed by 
the two points. If the process determines there is an 
intersection, the value of t is used in Equation 1 to 
calculate the intersection point and the process returns the 
point to the calling function. Table VI shows the source 
code I developed for the process. The source code is also 
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in Appendix C, which contains the source code of functions 
which deal with the three-dimensional environment, 



Table VI - INTERSECTION POINT SOURCE CODE 



point intersection_point (point ptl, point pt2, 

equation plane) 



{ 

double 



y = 0.0, t = 0.0, w = 0.0; 



y = (plane. x * (pt2.x - ptl.x) + plane. y * 

(pt2.y - ptl.y) + (plane. z * (pt2.z - ptl . z ) ) ; 

if ( !y) { /* Y == 0 */ 

intersection . x = NO_INTERSECTION; 

} 

else{ 

w = -(plane. x * ptl.x + plane. y * ptl.y + plane. z * 
ptl.z + plane. d); 
t = w / y; 



(0.0 <= t && t 


<= 1.0) 


{ 








intersection . x 


= ptl.x 


+ 


( pt2 . x 


- ptl.x) 


* t; 


intersection . y 


= ptl.y 


+ 


(pt2 . y 


- ptl.y) 


* t; 


intersection . z 


= ptl . z 


+ 


(pt2 . z 


- ptl.z) 


* t; 



} 

else 

intersection . x = NO_INTERSECTION; 

} 

return (intersection); 



e. Inside Polyhedron Check. 

If the process determines that there is a valid 
intersection point, it then determines if the point lies 
within the boundaries of the polyhedron. To determine this, 
the process projects a ray from the point to a point outside 
the search space, but lying on the same x y z plane as the 
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polyhedron face. The process then determines the parametric 
equations of this ray. Next, by finding the parametric 
equations for each boundary segment of the polyhedron face, 
we can equate the two sets of parametric equations. Since 
there are two unknowns (both parameters), the process uses 
two of the three equations to simultaneously solve for the 
unknowns . 

For example, if the points of the ray are 
( u i, v x , w 2 ) and (u 2 , v 2 , w 2 ), the parametric equations are 
shown in Equation 14 . 
x = u x + (u 2 - u x ) s 

y = v 1 + (v 2 - v x )s Eq 14 

z = w 1 + (w 2 - wj s 



Similarly, if the two end nodes of the boundary of the 
polyhedron face are (a x , b x , c x ) and (a 2 , b 2 , c 2 ), the results 
are shown in Equation 15 . 
x = a x + (a 2 - a x ) s 

y - b x + (jb 2 - jb x ) s Eq 15 

z = c x + (c 2 - c x ) s 

If we use the first two equations of Equations 14 and 15, we 
can solve for s and t. Equations 16 and 17 depict the 
results . 

= ((*?! - vj) (a 2 - a x )) - ( (a x - u x ) (i? 2 - bj ) 16 

( (a 2 - a x ) (v 2 - v x ) ) - ((£> 2 - Jb x ) (u 2 - u x ) ) 

= ((i? x - Vi) (u 2 ~ “i)> - ((*! ~ Qj) (v 2 - vj)) Ea 17 

( (a 2 - a x ) ( v 2 - v x ) ) - ( (2 j 2 - bj (u 2 - u x ) ) 

If 0 < s < 1 and 0 < t < 1 are valid statements, 

then the ray intersects the polyhedron face. This process 
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must be done for each boundary of the face with a counter 
counting the number of times the ray intersects the 
boundary. If the counter is an even number, then the 
intersection point is outside of the boundaries of the face 
and thus the projection from the expansion point to the goal 
does not intersect the polyhedron face. Conversely, if the 
count is odd, the start goal ray intersects the polyhedron 
face . 



An easy implementation of determining if the 
counter is odd or even is to use modulo arithmetic. The 
pseudo language in Table VII depicts this process. The 
source code can again be found in Appendix C. 

Table VII - PSEUDO LANGUAGE FOR POLYHEDRON INTERSECTION 



if ( line_intersection_count modula 2 == 1) 
polyhedron_intersection = True 



else 



end if 



polyhedron_intersection = False 



then 



As mention above, the process checks each face of 
each polyhedron. If there is not an intersection between 
the vector and any of the polyhedron faces, the calling 
process is notified by passing it a non-intersection flag. 
However, if there is an intersection, the process stops 
checking the remaining faces and notifies the calling 
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process of the intersection so that the path planning 
process can begin. 

2 . Plane Types 

The use of the plane is used several times 
throughout the process of finding the best path. There are 
two different types of planes used in finding the best path. 
I refer to one of the two types of planes used as a 
"vertical plane" . The other plane is the "horizontal 
plane" . I refer to a vertical plane as such because two of 
the three points lie on the same X Z plane. I refer to a 
horizontal plane as such because two of the three points are 
on the same X Y plane and it is perpendicular to the 
vertical plane formed by the same expansion and end points . 
The process of finding the equation for both types are the 
same and uses the same process described earlier in this 
chapter. Appendix C contains the source code which finds 
both of these planes . 

3 . Two-Dimensional Polygons 

Two-dimensional polygons are the type of obstacles 
which are used primarily for the path planning process. The 
general concept in building the two-dimensional world, the 
Build two d p olygonlist function, cycles through each face 
of each polyhedron, finding the intersection points of the 
horizontal plane and the line segments of the polyhedron 
face if one exits. If an intersection does exist, then the 
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process puts the point into a link list for the two- 
dimensional list. Figure 3 depicts the structure of this 
link list and Table VIII shows the pseudo language used to 
develop the list. The source code can be found in Appendix 
B. 



POLYGON START 




Figure 3 - 2D Polygon List 



The Next Polygon entry is a pointer which points at 
the next polygon in the linked list. The Polyhedron Number 
entry is the number of the polyhedron which the algorithm 
used to find the polygon. The Polygon Number entry is the 
number associated to when the polygon was formed. The 
Polygon Start entry is a pointer which points to the first 
node or vertex of the polygon. The nodes are in a doubly 
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linked list with "ccw" depicting counter clockwise and "cw" 
meaning clockwise in order. 



Table VIII - BUILD TWO-DIMENSIONAL POLYGON 



Build_two_d_polygon_list ( PolyhedronList , Plane ) 

{ 

initialize f irst_polygon ; 
current_polygon = f irst_polygon 
for (Each Polyhedron in PolyhedronList)! 
for (Each Face on Polyhedron)! 

for (Each Line Segment of the Face)! 

if (( intersection = intersection_point (nodel, 

node2 , plane) != NO_INTERSECTION) 
Create_polygon ( intersection , 

current_polygon ) ; 

} 

} 

remove_duplicate_node ( current_polygon ) ; 
initialize next_polygon ; 
current_polygon = next_polygon; 

} 

return (polygon_list ) ; 



4 . Number and Types of Paths 

In the two-dimensional environment, the number of 
possible paths is 2 n where n is the number of obstacles in 
the environment. As already stated, I do not consider paths 
which go underneath obstacles. As a result, the number of 
possible paths in the three-dimensional environment is 3 n . 

In using Kanayama's terminology where a plus sign (+) 
denotes a path extending counter clockwise around an 
obstacle and a minus sign (-) denotes a path extending 
clockwise, I extend it to include an up arrow (T) which 
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denotes a path extending over the obstacle. Table VII shows 
the possible paths for an environment with 1, 2 and 3 
obstacles . 

In analyzing these possible paths in Table IX, one 
can see that general paths with no up arrows indicate that 
the path lies on the same plane as the start and goal. In 
addition, there is one path which extends above all the 
obstacles. Thus, all other paths are a combination of the 
two. In further analysis of Table IX, these paths can be 
generalize to include all cases. These cases are: 

1. The path lies on the same plane as the start and goal. 

2. The path goes over all the obstacles. 

3 . The path goes over one or more obstacles before going 

around the remaining one or more obstacles . 

4 . The path goes around one or more obstacles before going 

over the remaining one or more obstacles. 

5. The path goes over one or more obstacles, goes around 

one or more obstacles and then goes over the remaining 
one or more obstacles or begins the sequence of around 
and back over the remaining obstacles until the path 
reaches the goal. 

6. The path goes around one or mo e obstacles, goes over 
one or more obstacles and then goes around the 
remaining one or more obstacles or begins the sequence 
of over and back around the remaining obstacles until 
the path reaches the goal. 

I could argue that bullets 3 and 4 are subsets of 
bullets 5 and 6 respectively. However, the concept of 
three-dimensional path planning is a difficult one. 
Therefore, I have broken the types of paths as above, 
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allowing me to explain in a more forthright and detailed 
manner the process I developed to find the best path. 



Table IX - POSSIBLE PATHS FOR POLYHEDRON WORLD 



1 OBSTACLE 


2 OBSTACLES 


3 OBSTACLES 


1A1 


LAi B) 


(Af Bjr C ) 


A+ 


A+B+ 


A+B+C+ 


A- 


A+B- 


A+B+C- 


At 


a+bT 


a+b+cT 




A-B+ 


A+B-C+ 




A-B- 


A+B-C- 




a-bT 


a+b-cT 




aTb+ 


a+bTc+ 




aTb- 


a+bTc- 




aTbT 


a+bTcT 






A-B+C+ 

A-B+C- 

A-B+Ct 

A-B-C+ 

A-B-C- 

a-b-cT 

a-bTc+ 

a-bTc- 

a-bTcT 






aTb+c+ 

aTb+c- 

aTb+cT 

aTb-c+ 

aTb-c- 

aTb-cT 

aTbTc+ 

aTbTc- 

aTbTcT 


TOTAL POSSIBLE PATHS 






3 


9 


27 
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5 . Tangents 

As with Kanayama's two-dimensional path planning 
algorithm, my process uses tangent lines extended from the 
start point or an expansion point to all valid obstacles. 
However, my algorithm extends these tangent lines to not 
only the sides of the obstacle, but also to the top of the 
obstacle . 

These two basic tangent types the process uses in 
the path planning process can be classified as either 
horizontal or vertical tangents. I further sub-classify the 
horizontal tangent as either a plus tangent or a minus 
tangent . 

a. Plus and Minus Tangents 

I define a tangent to a three-dimensional 
polyhedron to be the ray which is formed by the expansion 
point and an intersection point on an edge of the polyhedron 
such that the ray intersects the polyhedron only at the 
intersection point. For the case where a tangent crosses a 
face of a polyhedron, then the first intersection point is 
the considered the tangent point. As in Kanayama's 
algorithm for the two-dimensional environment, I define a 
plus tangent to be a tangent which if it was extended around 
the obstacle so that it encircled the obstacle, it would 
wrap around the obstacle in a counter-clockwise direction. 
Conversely, a minus tangent is a tangent which if the 
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tangent encircled the obstacle, it would wrap around the 
obstacle in a clockwise direction. Figure 4 shows the 
different plus tangents for a two-dimensional polygon. 
Similarly, Figure 5 shows the different minus tangents. 





Up until now, I have shown the plus and minus 



tangents for a two-dimensional polygon. But as Figures 6 
and 7 show, there are an infinite amount of plus and minus 




Figure 6 - Plus Tangents for Figure 7 - Minus Tangents for 



Polyhedron World Polyhedron World 

tangents when dealing with a three-dimensional environment. 
However, this unmanageable amount can be reduced to a more 
manageable amount determined by the type of path being 



38 



expanded on and the number of polyhedrons within the 
environment . 

The general idea behind this reduction is to find 
the polygon formed by the intersection of a plane and the 
polyhedron. By using this plane to find all the polygons 
formed by the intersection of the plane and the polyhedrons 
in the environment, the three-dimensional world is reduced 
to a two-dimensional representation. Of course, the number 
of plane slices needed will be different for each of the 
types of paths as listed above. See Figure 8 for a 
graphical representation of this intersection. 




I use Kanayama's basic algorithm for finding each 
of the type of tangents from a point to a polygon. However, 
I expand on it to find only the closest tangent point to the 
expansion point. This case must be accounted for when the 
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expansion point and one of the sides of the polygon lie on 
the same projected ray. Table X shows the pseudo language 
Kanayama has developed to find the plus tangent (Kanayama 
90). The minus tangent code is the same except that the 
"if" structure which checks the sign and order is evaluated 
with -1 and not 1. The sign function accepts an integer 
which the order function returns and returns either a 1 or - 
1 based on whether or not the function is positive or 
negative. The order function is a modification of the area 
of a triangle equation. The function determines the order, 
either clockwise or counter-clockwise, of the three points. 
For further discussion, see Kanayama 's development of the 
function (Kanayama 90). 



Table X - PLUS TANGENT PSEUDO LANGUAGE 



plus_tangent (pt , polygon_A) 

{ 

q = initial_node(polygon_A) ; 
dof orever { 

if (sign(order (pt, q, next(q)) == 1 ) q = next(q); 
else{ 

if (sign (order (pt, q, prev(q)) == 1 ) q = prev(q); 
else break; 

} 

} 

return (q ) ; 



In determining if the expansion point lies on the 
same project ray as a side of a polygon, the process must 
make two checks. The first check determines if the 
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direction of the ray formed by the expansion point and the 
point found by the plus or minus tangent function is equal 
to the ray formed by the expansion point and the vertex node 
counter-clockwise to the expansion point. Similarly, the 
second check determines if the ray formed by the expansion 
point and the vertex node clockwise to the point is equal to 
the ray formed by the tangent point and expansion node. If 
the rays are equal, then the vertex node which is closest to 
the expansion point is determined and returned. Figure 9 
shows these scenarios. Table XI shows excerpts of the C 
code I developed to accomplish this. Appendix I gives the 
complete source code for both the plus and minus tangent 
functions . 




Figure 9 - Direction Heuristic Check 
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Table XI - DETERMINING CLOSEST TANGENT POINT SOURCE CODE 




The case of finding the tangent from one polygon 
to another polygon needs to be handled in a different 
manner. The problem encountered in finding this type of 
tangent happens when the tangent node from the first polygon 
is not the point on the polygon which the process is 
expanding from. Figure 10 depicts this case. 

As can be seen in Figure 10, if the plus tangent 
is found using the expansion point, the tangent line 
(Expansion Point, A) cuts across the first polygon, which 
cannot occur for a valid partial path. The same thing 
occurs when trying to find a minus tangent. As a result, I 
had to develop a process which found the appropriate 
tangents between the polygons (tangent lines (C,B) and (E,D) 
respectively) . 
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Polygons 

In finding the two appropriate tangent nodes on 
the polygons, I had to consider what type of tangent was 
used in arriving at the expansion point. The idea behind 
the process is then to find the tangent node on the second 
polygon from the expansion point. This tangent node is then 
used in finding the tangent node on the first polygon. This 
process of oscillating from one polygon to the next polygon 
continues as long as the tangent line intersects one of the 
two polygons. Upon finding the two points the process 
returns the two nodes in a link list to the calling 
function. Table XII depicts this process in pseudo language 
for each of the different cases in finding the appropriate 
tangent. Appendix I contains the function "tangent" written 
in the C language, which also has all the cases. 
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Table XII 



FINDING APPROPRIATE TANGENTS 



polygonl_pt = expans ion_pt; 

/*expansion pt tangent is minus 
plus tangent to polygon2*/ 
do { 

polygon2_pt = plus_tangent (polygon2 , polygonl_pt ) ; 
polygonl_pt = plus_tangent (polygonl, polygon2_pt ) ; 

) while (not (visible (polygonl_pt, polygon2_pt ) ; 

/*expansion pt tangent is minus 
minus tangent to polygon 2*/ 
do { 

polygon2_pt = minus_tangent (polygon2 , polygonl_pt ) ; 
polygonl_pt = plus_tangent (polygonl, polygon2_pt ) ; 

Jwhile (not (visible (polygonl_pt, polygon2_pt ) ; 

/*expansion pt tangent is plus 
plus tangent to polygon 2*/ 
do { 

polygon2_pt = plus_tangent (polygon2 , polygonl_pt ) ; 
polygonl_pt = minus_tangent (polygonl, polygon2_pt ) ; 

Jwhile ( not (visible (polygonl_pt , polygon2_pt ) ; 

/♦expansion pt tangent is minus 
minus tangent to polygon 2*/ 
do { 

polygon2_pt = minus_tangent (polygon2 , polygonl_pt ) ; 
polygonl_pt = minus_tangent (polygonl, polygon2_pt ) ; 

Jwhile ( not (visible (polygonl_pt, polygon2_pt ) ; 



b. Vertical Tangents 

Compared to finding the plus and minus tangents, 
finding the vertical tangents is a relatively simple 
exercise. Since I only am concerned about the top vertical 



44 



tangent of each polyhedron, I refer to this tangent as the 
vertical tangent. Figure 11 depicts the vertical plane, 
the obstacle, and the tangent. 




Figure 11 - Vertical Tangent 

Unlike the functions plus_tangent and 
minus_tangent , which find a point which forms the tangent, 
the process I developed finds not only that one point which 
forms the tangent, but it also finds the intersection point 
of the vertical plane which is opposite to the side of the 
top polyhedron face from which came the first point. Figure 
12 depicts the scenario. I include both intersection points 
because if the vertical extension of a path is to go over 
only one polyhedron, then the second point will eventually 
be needed. Since the vertical plane is already determined, 
then it is logical to expand the path to the second point 
and not the first. In addition, the process finds all 
vertical points to all polyhedrons lying between the 
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expansion point and the end point. In the event that there 
are more than one obstacle, then I have also included a 
check which determines if points are visible not to the next 
point in the list, but to the other points in the vertical 
list as depicted in Figure 13. This allows me to exclude 
all intersection points not needed in developing the 
extended path. Appendix J contains the source code for 
finding these vertical points. 




One Polygon 




Figure 13 - Vertical Nodes for 



More Than One Polygon 



6. Path Representation 

In designing my algorithm for the path planning 
process, I had to develop a structure which I could 
represent the path. This path structure evolved through 
many iterations until I arrived at the final structure. 
Figure 14 shows the structure and the file 3d_tan.h is in 
Appendix L contains the source code defining the structure. 
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PAHllAL PATHS 



NEXT PATH 




NEXT PATH 


ABLE2 EXPAND 




ABLE2 EXPAND 


COST 


COST 


ESTIMATED COST 


ESTIMATED COST 


TOTAT. COST 


TOTAL COST 


RAY DIRECTION 


RAY DIRECTION 


TYPE TANUENT 


TYPE TANUENT 


PLANE NUMBER 


PLANE NUMBER 


WHEN PATH KXlND 


WHEN PATH FOUND 


FORWARD 


FORWARD 


POLYGONS VERT 




ll 


POLYGON4VERT 


POLY OON4 VERT ► 


POLYUON4VERT ► 




last.node 


LAST.NODE 



POINT 




POINT 


NEXT NODE 




NEXT NODE 



POINT 



NEXT NODD 



Figure 14 - Partial Path List 

The Next_path pointer and the Last_node pointer in 
the list are self-explanatory. The 3 cost entries are used 
in the search function to identify the shortest path to 
expand the path. The cost entry is the cost from the start 
point traveling along the path to the last node added to the 
goal; the estimated cost entry is the euclidean distance 
from the last point added to the goal; and the total cost is 
the summation of the other two costs . The ray direction 
entry is the direction on the X Y plane of the tangent from 
the second to last node to the last node added to path. 

This entry is used in a direction heuristic, which is used 
to prune possible path entries. The type_tangent entry is 
the type of the last tangent formed by the last node and the 
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second-to-last node. The entry "Plane number" is the number 
of the horizontal plane slice used in finding the path 
around obstacles . This entry ensures that the same plane is 
used to expand a path around the obstacles . The able2expand 
entry is a flag used by one of the heuristic to determine if 
the path should be extended or not. The integer forward 
identifies the path as built from the start to goal or from 
the goal to the start. Polyhed4vert identifies the polygon 
which the path will go around after going over the preceding 
obstacles. The Polyhed4vert_PTR is a pointer which points 
to an X Y node structure and is used to expand the path from 
a vertical node around an obstacle and over other obstacles 
(path types 5 and 6). I refer to the information in this 
structure as the header information of the path. 

7 . Active Node List and Active Paths Heuristic 

The Active Node List is a link list which identifies 
all those nodes which have been added to the paths in the 
partial path list. Figure 15 shows the structure I use in 
this link list. As with the partial path structure, the 
source code for this structure is found in 3d_tan.h in 
Appendix L. Along with the X Y Z point, the structure also 
has an entry for the cost of the path from start to this X Y 
Z point and an integer which corresponds to when the path 
was found. 
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Figure 15 - Active Node List 



This linked list is used in a heuristic which 
determines if a path being expanded from has been visited or 
not. If the point has already been visited, the distance 
entry in the structure is compared with the distance from 
the start to the last node in the expanded path plus the 
distance from the last node to the X Y Z point. If the 
distance in the structure of the link list is less than the 
computed distance, then the new point will not be added to 
the link list as an active node and the path will not extend 
to the this point. If the distance in the list is greater 
than the computed distance, then the new point is added to 
the link list as an active node and the path will be 
extended to the new point. In addition, the partial path 
with which the entry, when_path_found, equals the entry in 
the list, then the entry, able2expand, is marked as not 
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extendable. Also the distance and when_path_f ound entries 
are updated to reflect the data for this new added node to 
the partial path. If the node has not been visited in the 
path planning process, the node with its corresponding data 
is added to the active node list and the partial path is 
extended to that point. Table XIII depicts the pseudo 
language for this heuristic and the source code is in 
Appendix D. 



Table XIII - CHECK ACTIVE NODE PSEUDO LANGUAGE 



check_active_node_list (node_list , shortest_path, pt, 

new_when_path_f ound) 

{ 

for (each active_node) { 

if (active_node->pt == shortest_path->last_node) { 
if (active_node->distance > shortest_path->cost + 
distance ( shortest_path- >last_node, pt) { 
mark_old_path_unexpandable (when_path_f ound ) ; 
update_act ive_node ( new_dis tance , 

new_when_path_f ound ) ; 
return ( EXPAND ) ; /*EXPAND == 1*/ 

} 

else return ( NOT_EXPANDABLE ) ; 

} 

} 

/♦node not visited yet*/ 

insert_node_into_node_list (node_list, pt, cost); 
return (EXPAND) ; 



8 . 



extend. 



Visibility 

After the algorithm has found the shortest path to 
the algorithm first determine if the last node added 
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to the path is visible either the goal or start point. This 
case is different from the visibility check used for the 
start and goal points because the last node can be on the 
backside of a polyhedron from the goal and still be visible 
with the goal as long as other polyhedrons lie between the 
last node and the goal. Figure 16 depicts this case. The 
concept of finding the path around the obstacle appears 
relatively simple; however the code is not. Table XIV lists 
the pseudo language I developed to solve this problem. 

Figure 17 identifies the points associated with both 
tangents of the situation shown in Figure 16. 




Figure 16 - Last Node Goal 

Visibility 




Figure 17 - Points found for 

both Tangents 



51 



Table XIV - PSEUDO LANGUAGE FOR VISIBILITY 



Vis ibil ity ( expand_point , goal_point , 
shortest_path ) 



{ 



start, two d list, 



if ( shortest_path forward) 
goal = start; 
else goal = goal_point; 

if ( ! intersect_polyhedron ( expand_point , goal ) ) { 
add_goal_to_goal_list ; 
return ( goal_list ) ; 

if (shortest_path->type_tangent = VERTICAL) 
return ( NULL ) ; 
else{ 

f ind_polygon_last_node_is_on ; 

/*finds the plus tangent from goal to polygon 
for (each type of tangent from goal to the polygon) 
if ( PLUS_TANGENT ) 

f rom_polygon_pt = 

plus_tangent (polygon ->polygon_s tart , goal) ; 

else 

f rom_polygon_pt = 

minus_tangent (polygon->polygon_start , goal) ; 



/*checks for intersection*/ 

if ( intersect_polyhedron ( f rom_polygon_pt , goal ) ) 
return ( NULL ) ; 

else{ /*two points are visible*/ 

place goal and from polygon pt in goal list 
find from_polygon_pt on polygon 
if ( PLUS_TANGENT ) { 

cycle counter-clockwise 

add points to goal list until arrive at 
expand_point 

} 

else { 

cycle clockwise, add points to goal list 
until arrive at expand point 

} 

if ( direction_heuristic ( shortest_path, 

goal_list- >pt ) 

return ( goal_list ) ; 

] 

} 

deallocate and free memory of goal list 
return (NULL) ; 

} 

} 
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In the pseudo language for the function visible, I 
refer to a function called "direction_heuristic" . This 
function uses geometric concepts of polygons and rays to 
eliminate a path from extending incorrectly. If Figure 17 
is used as an example, the "direction_heuristic " ensures 
that the nodes associated with the minus tangent are not 
chosen, while the nodes associated with the plus tangent. 
Figure 18 depicts why points F and G would not be chosen 
since point B can extend directly to G. Appendix F contains 
the source code for the "direction heuristic" function. 



Q 




Figure 18 - Direction Heuristic 
Characteristics 

The "visibility" function returns to the main 
function the goal list. If the goal list is not NULL, then 
the while loop is exited and the algorithm will add the goal 
list to the shortest path, update the header information of 
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the shortest path, delete all other paths, and terminate. 

If the goal list is NULL, then the algorithm enters the loop 
structure to begin extending the best path chosen to extend. 
Prior to extending the path, the algorithm prints to a file 
or to the screen the paths that are in the partial path list 
and the path chosen as the shortest path to extend. 

B. THREE-DIMENSIONAL PATH PLANNING 

As stated above, my three-dimensional path planning 
follows along the same concepts as Kanayama's two- 
dimensional algorithm. The basic algorithm begins by 
prompting the user to input the start and goal points . Once 
the user inputs these points, the algorithm checks whether 
or not there are obstacles lying between the two points . 

If there are one or more obstacle lying between the start 
point and goal, the algorithm then finds the two dimensional 
representation of the environment using the horizontal plane 
formed by the start and goal points as discussed earlier in 
this chapter. After finding this two-dimensional 
representation, the algorithm begins to find all the initial 
partial paths. After the algorithm finds these initial 
paths, it searches the path list to find the path to expand 
from after the initial paths are found. Next, the algorithm 
checks to determine if either the goal for a forward type 
path or the start point for a reverse path is visible from 
the last node added to the partial path. If it is, the goal 
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is added to that path, the path is marked as a completed 
path, all partial paths are deleted and associated memory 
deallocated, and the process ends. If the goal or start 
point is not visible, then the path is extended to all valid 
points in the environment and the process begins again with 
searching for a new path to expand. Table XV shows this 
basic algorithm in pseudo language. 



Table XV - PATH PLANNER PSEUDO LANGUAGE 



Three-dimensional_path_planner ( ) 

{ 

Create_3D_world( ) ; 

Input_start_and_goal ( ) ; 

if ( ! intersect_polyhedron ( start, goal ) ) { 
build_best_path(start_goal ) ; 
return (best_path) ; 

} 

Two_D = build_two_dimensional_representation( start, 

goal ) ; 

Partial_paths = Find_initial_paths ( 3D_world, Two_D, 

start , goal ) ; 

while (visible ( f ind_shortest_path ( partial_paths ) , 
start_point, goal ) ) { 

expand_shortest_path( shortest_path, 3D_world, 

Two_D, goal , active_node_list ) ; 

} 

Add_goal_and_mark_completed ( shortest path ) ; 
Delete_partial_paths (partial_paths ) ; 

} 



1. Initial Paths 

In finding the initial paths, the main algorithm or 
the function "main (void) calls the functions 
"start_finding_paths" . This function accepts the polygon 
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list, the start point and the goal point and returns to the 
main function the initial partial paths. Table XVI shows 
the pseudo language I developed for the function. Appendix 
F contains the source code for this function. 

Table XVI - PSEUDO LANGUAGE FOR START FINDING PATHS 



start_f inding_paths ( 3d_world, polygons, start, goal) 

{ 

f ind_vertical_and_horizontal_planes 

polygons = build_2d_polygon_world ( 3d_world , 
horizontal ) 

for(each polygon in list){ 

ptl = plus_tangent (polygon_nodes , start); 
pt2 = minus_tangent (polygon_nodes , start); 
if (direction_heuristic_from_start (ptl, start, goal){ 
if (visible(ptl, start) 

create ne w p artial path(partial paths ) ; 
add_start_goal ( start , goal, new_path) 
update_header (new_path, goal); 
else 

partial_paths = up_and_over (start_point, 

3 d_wor 1 d , ptl); 

if (direction_heuristic_from_start (pt2 , start, goal){ 
if (visible (ptl , start) 

create_new_partial_path (partial_paths ) ; 
add_start_goal ( start , goal, new_path) 
update_header (new_path, goal); 
else 

partial_paths = up_and_over (start_point, 

3d_world, pt2 ) ; 

end for loop 

partial_paths = up_and_over ( start , goal, 3d_world); 



The function " start_f inding_paths " begins by finding 
the vertical and horizontal planes formed by the start and 
goal. It then proceeds to find the polygons formed by the 
intersection of this horizontal plane and the polyhedrons in 
the world. After finding this polygon list, the process 
starts with the first polygon in the list and finds both the 
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plus and minus tangent points with respect to the start 
point as described earlier in this chapter. The process 
then uses a heuristic to determine if the direction of the 
ray formed by the start and goal is within plus or minus 180 
degrees of the ray formed by the start and goal points. If 
this heuristic determines that the ray is valid, then the 
process continues by making additional checks. If the 
heuristic determines that the ray is not valid, the process 
stops checking the plus tangent point and begins the process 
over checking the minus tangent point. 

Once the process determines if the ray is valid, it 
begins to check if the start point is visible with this plus 
tangent point. The process uses the polygon list to 
determine if the two points are visible. If a two points 
are not visible using this two-dimensional polygon list, 
then the two points will not be visible using polyhedron 
list since the polygon list is a subset of the polyhedron 
world. By using this smaller polygon list, the process can 
check for visibility faster than if it used the polyhedron 
list . 

In determining that the two points are visible, the 
process begins to find a partial path which corresponds to 
the type of path in which all nodes are on the same plane. 

If the two points are visible then the process adds a new 
partial path to the partial path list. This partial path 
list has the start point as the first point and the plus 
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tangent point as the last_node. In addition, the process 
also computes the header data of the partial path. 

If the process determines that the two points are 
not visible, then the process begins the phase of finding 
the type of path which goes over one or more polyhedrons and 
then around (at least one) the rest of the polyhedrons to 
the goal. 

In finding the this path, the process calls the 
function " up_and_over " , passing to the function the partial 
paths pointer, expansion point, start point and the either 
the plus or minus tangent point, depending on which point is 
being checked. The function returns the partial path list 
adding to the list the vertical path as described earlier in 
this chapter. The function up_and_over and the functions 
this function calls can be found in Appendix J for further 
analysis . 

After each polygon in the two-dimensional list has 
been checked, the "start_f inding_f unction" then finds the 
partial path which travels over all the polyhedrons lying 
between the start and goal. It does so by using the same 
function used above. In using the "up_and_over " function, 
the calling function passes the same parameters, except the 
goal point is passed instead one of the two tangent points. 

The last step the process determines is to find the 
paths which go around one or more polyhedrons and then over 
(at least one) the remaining polyhedrons. This step is not 
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as difficult as it appears. Upon analyzing this step, I 
noticed that this step was the similar to the step of 
finding all the paths which go over one or more polyhedrons 
and around the remaining only in reverse order. As a 
result, to find these type of paths, I developed a sub- 
process which extends from the goal and not the start point. 

The "start_f inding_paths " function calls the 
function "up_and_over_f rom_goal " . I developed this function 
along the same lines as "start_f inding_paths " . For each 
polygon in the two-dimensional list, the sub-process finds 
both tangents points with respect to the goal, the expansion 
point. After finding the these points, the sub-process 
first determines if the expansion point is visible to the 
plus tangent point. If the points are not visible, the 
"up_and_over " function is called, adding paths extended from 
the goal. After checking the plus tangent point, the sub- 
process then performs the same operations for the minus 
tangent point. To distinguish these paths from the paths 
extended from the start point, the "forward" integer flag in 
the header information of the path is flagged equal to 0 
while this flag is equal to 1 if the path extends from the 
start. Upon returning control back to 

"start_finding_paths" , the function returns the partial path 
list, which in turn returns the same list back to the main 
function. As with the other vertical function, the 
"up_and_over_from_goal" function is listed in Appendix J. 
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When control of the process returns to the main 
function, all the partial paths which are needed to find the 
best path are included in the list. From this list, the 
process searches for the best path from which to extend 
from. 

2 . Extending Partial Paths 

In Extending a partial path, the algorithm enters a 
while loop structure which first searches the partial path 
list to find the best path to extend, secondly determines 
the visibility of the last node of the best path chosen, and 
thirdly, based on the results of checking the visibility, 
extends this path or exits the loop, 
a . Search Technique 

After finding these initial paths, the process 
uses an A* (A Star) search to determine which path to 
extend. When considering the various paths for expansion, 
the A* search uses the function f(n) = g(n) + h(n) where 
g(n) is the actual cost of the path from either the start if 
a forward path or the goal if a reverse path; h(n) is the 
heuristic estimate or the cost/distance from either the last 
node to the goal if the path is a forward path or the last 
node to the start point if the path is a reverse path. In 
the header data of each path g(n) is the "cost" entry; h(n) 
is the "estimated cost" entry; and f(n) is the "total cost" 
entry. The minimum f(n) of all possible paths identifies 
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the shortest partial path and determines which path the 
algorithm will use to extend to the next set of nodes. 

The advantage of using the A* Search is that the 
heuristic measure, h(n) is a lower bound of the actual cost 
of the shortest path. As a result, the algorithm is 
admissible, finding an optimal path if one exists. Once the 
A* Search has determined which path to extend, the algorithm 
begins a process similar to the one used to find the initial 
paths . 

b. Extending the Shortest Path 

Extending the shortest path begins with the 
function " extend_path" . The main function passes to 
" extend_path" the following parameters: start point; goal; 

and pointers to the shortest path, polygon list, polyhedron 
list and the active node list. It returns to the main 
function the pointer to the shortest path. 

Table XVII lists the pseudo language I developed 
to extend the path. As can be seen, the concept behind the 
function is to check the type of tangent entry in the header 
information of the shortest path. If the type tangent is 
not a vertical tangent, then the path is extended on the 
plane slice indicated by the plane number entry in the 
header information. However, if the type_tangent entry is 
vertical, then a new plane slice must be used. In addition, 
the function is Appendix F. 
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Table XVII 



PSEUDO LANGUAGE FOR EXTEND PATH 



extend_path 

{ 

if ( shortest_path-> type_tangent != VERTICAL) 
f ind_tangents_from_inner_nodes ; 

else{ 

add_on_new_plane_intersection; 

continue_vertical_path; 

} 

return ( shortes t_path ) ; 

} 

c. Vertical Extension 

In extending the path in which the tangent to the 
last node of the path is a vertical tangent, the process 
first finds the horizontal plane formed by the last node of 
the path and the goal. Then the process finds the 
intersection of the plane with each polyhedron in the world. 
These new polygons are appended to the list of the existing 
polygons with the plane_number entry corresponding to the 
number of horizontal planes used finding polygon 
intersections . 

After finding the polygon intersections for 
the new horizontal plane slice, the process must accomplish 
two tasks. The first task is the find the type of paths 
which correspond to the last two types of paths as referred 
to earlier in the chapter. The second task is to extend the 
path to the one node on the horizontal plane slice found 
directly before beginning this process . 



62 



To find the paths which begin traversing over 
one or more obstacles, then around one or more obstacles and 
then over at least one obstacle or similarly around, over 
and then around are found from a vertical point. As with 
the paths which correspond to the third and fourth types of 
depending on the direction of the path. If the direction of 
the path is forward, then the type of path found is up, 
around and up; however, if the direction is from the goal, 
the type of path is around, up, and around. Since finding 
these two types of paths are the hardest to find, I left 
them for the last task to accomplish and at the time of 
writing of this paper I have not implemented the process 
into the algorithm which I developed. However, Table XVIII 
shows the process written in pseudo language. 
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Table XVIII 



PSEUDO LANGUAGE FOR FUNCTION UP, OVER, UP 



up_over_up ( path ) 

{ 

For (each polyhedron)! 

from = find top f ace ( lst_polyhedron ) ; 
for (each polyhedron not the 1st polyhedron)! 
to = find top face ( 2nd_polyhedron ) ; 
for (loop 4 times) /*there are 4 tangents to 

find*/ 

tangent_list = tangent (to, from, model, mode2 ) ; 
if ( check_tangent_list ( tangent_list ) t 
f ree_tangent_list ( tangent_list ) 
continue; 

} 

vertical_list = find_vertical_list ( tangent_ptl, 
tangent_pt2 ) ; 

last_node = find last node in vertical_list ; 
new_poly_list = build two-d polygon list 

(last_node, path's 
last_node) 

ne w p ath list = duplicate_path(path) 
new_path_list = continue_vertical_path using 
n ew_p ol y_l i s t 
while ( not (visible ( 

f ind_shortest_path(new_paths_list ) , 
new_poly_list , tangent_Ptl) 
new_path_list = extend_path(new_path_list , 

tangent ptl,new poly list ) ; 
/*tangent_ptl is new goal point*/ 

} 

add tangent_ptl to shortest path; 
using this process' shortest path 

if (direction heuristic with last node in path 
and last node in vertical_path ) 
add vertical list to shortest path 
else mark path as not able2expand 
delete all partial paths in new list except 
shortest 

place in locally shortest_path_list 
f ree_new_poly_list ( new_poly_list ) ; 

} 

} 

add locally shortest path list to partial path list 
after passed in path and connect links 
return (path) ; 

} 
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In beginning the second task of the process, the 
process uses these new polygons and finds the one node which 

extends the path along the same ray direction in the header 

information. In doing so, the process scans these new 
polygons checking the polyhedron entry. If this entry 
equals the entry of polyhed4vert , then the process finds the 
two horizontal tangents from the last node to the polygon. 
The process then determines the rays of these two tangents. 

The process adds to the path as the last node the tangent 

point whose tangent direction is in the same general 
direction as the ray_direction entry in the header 
information . 

In determining if the two rays are in the 
same general direction, I had to consider the case where the 
first plane slice created a polygon with x and y entries in 
the link list that are different from the x and y entries in 
the link list of the polygon formed by the new plane slice. 
Since the first plane and its associated polygons were used 
to determine the vertical points of the path, the new 
polygon used to determine the tangent point will result in a 
different path if this tangent point and start point are 
used . 

In determining if the point should be added, 

I modified the process to first check the type_tangent entry 
in the structure which the pointer polyhed4vert_PTR is 
linked to. The process then executes the correct tangent 
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function (either plus_tangent or minus_tangent ) . The next 
check the process does is to determine if the difference in 
ray directions is less than .0001. Instead of using zero, I 
use this value since this algorithm may be used on different 
platforms and the precision of the platforms may be 
different. If the difference is, then the point is added to 
the list. If the difference is not, then the process must 
recalculate the vertical intersection points using the 
vertical plane formed by the start point and this new point. 
In both cases, the header information is updated accordingly 
and the function returns to calling function, extend_path, 
the shortest path. The function "continue_vertical_path" is 
in Appendix J. 

(1) Horizontal Extension 

If the function " extend_path" determines that 
the shortest_path' s type_tangent entry does not indicate a 
vertical tangent, then the process begins to find all valid 
tangents on the plane slice indicated by the plane_number 
entry in the header information. The function which 
performs this work is "f ind_tangents_from_inner_nodes " and 
the source code is in Appendix F. 

The main concept behind this function is to 
cycle through the polygon list. For those polygons which 
have the same plane number as the shortest path's 
plane_number entry, the tangent formed by the polygon being 
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checked and the polygon which the last node of the shortest 
path lies as described earlier in this chapter. 

The process continues on to check whether the 
ray formed by the two tangent points intersects any of the 
polyhedrons in the world. If there is not, then the 
shortest path is duplicated entirely, the two tangent points 
are added to the duplicated path. In addition, all points 
which lie on the same polygon as, are between the shortest 
path's last node and one of the tangent points, and are in 
the direction (either plus or minus) the path is advancing 
are added to the path between these two points. After 
completing the duplication and insertion of the new points, 
the header information is updated and the process inserts 
the new partial path into the partial path list after the 
shortest path, reconnecting the link list accordingly. 

C. FINDINGS AND EXAMPLES 

As I have already indicated, the number of paths grows 
in polynomial time and is dependent not only on the number 
of polyhedrons but also the number of faces in each 
polyhedron and number of nodes in each face. As a result, I 
will limit the number of polyhedrons in the environment to 1 
and 2. In addition, I have included a print function which 
prints out each partial path listing prior to an iteration 
of extending the shortest path. A listing of for each 
example is in Appendix M. 
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1 . One Polyhedron World 

For the 1 polyhedron world, I use the polyhedron 
described in Table XIX. In the first example, I use the 
starting point (1, 15, 14) and the goal point (40, 15, 14). 

I use the same depth value (z value) in the example to show 
that the plane slice does correctly work. Figure 19 depicts 
the scenario with a top down view for this example. 

In generating the initial partial path list, the 
algorithm calculated 3 partial paths. Using the same top 
down view of Figure 19, Figure 20 shows the partial paths 
with nodes of the path labeled. 

The reader should notice that the nodes of the path 
which end at a vertex of the polygon in Figure 20 are nodes 
of the polygon found using the horizontal plane formed by 
the start and goal points . These two paths also represent 
the type of paths in which all nodes of the path lie on the 
same plane. Similarly, the one path in Figure 20 which does 
not have a tangent to the polygon as a node of the path 
represents the path which traverses over all polyhedrons to 
the goal. In addition, the intersection points in Figure 20 
represent the intersection of the vertical plane and the top 
face of the polyhedron. 
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Table XIX - ONE POLYHEDRON WORLD 



SIDE FACE Is (10 7 15) (10 20 15) (10 20 3) (10 7 3) 

SIDE FACE 2: (10 20 15) (14 20 15) (14 20 3) (10 20 3) 

SIDE FACE 3: (14 20 15) (14 7 15) (14 7 3) (14 20 3) 

SIDE FACE 4: (14 7 15) (10 7 15) (10 7 3) (14 7 3) 

TOP FACE: (10 7 3) (10 20 3) (14 20 3) (14 7 3) 

BOTTOM FACE: (10 7 15) (10 20 15) (14 20 15) (14 7 15) 





The algorithm chose the path (1 15 14) to (10 20 14) 
to extend using the A* search. In checking the visibility 
of the point with the goal, the algorithm determined that 
the expansion point was visible. As a result the correct 
node(s) of the polygon were added to the path along with the 
goal, the header information of the path updated, all other 
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paths, the polygon list and the polyhedron list are deleted 
from memory and the algorithm terminates with the best path 
as (1 15 14) to (7 20 14) to (14 20 14) to (40 15 14). 

In the second example using the one polyhedron 
world, I used the points (1 13 5) and ( 1 40 14) for the 
start and goal points respectively. From the top down view, 
these initial partial paths look similar to the paths in the 
previous example; however, since the start and goal points 
are not at the same depth, the intersection of the 
polyhedron and the horizontal plane produces a polygon with 
different depth values than the first example as shown in 
Figure 21. 
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Figure 21 - Second Example for 
1 Polyhedron 



In this example, as expected, the shortest path is 
the vertical path. Since the vertical path is directly 
visible to the goal, the algorithm adds the goal to the path 
list and again, it performs all the administrative functions 
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to update the header information, deletes all appropriate 
paths and terminates . 

2 . Two Polyhedron World 

For the 2 polyhedron world, I use the same 
polyhedron as in the examples for a one world polyhedron. 
Table XX lists the coordinates of the nodes of the second 
polyhedron. In addition, I use the same start and goal 
points in both examples, similar to the first two examples. 
Figure 22 shows the initial partial paths . As can be seen 
and expected, the number of paths has increased with the 
process including of the vertical paths over the first 
polyhedron. In addition, since the second polyhedron is 
taller than the first polyhedron (z values are smaller, thus 
at a shallower depth). Figure 22 also shows those nodes of a 
path marked with an "x" . With respect to the one vertical 
path from start to goal, close observation will show that 
the one node on the edge of the top face closest to the 
second polyhedron was not included in the path. This node 
is not needed since the nodes in between this node are 
visible to each other. As in the first example, the process 
chose the partial path (1 15 14) to (10 20 14) as the path 
to extend. Figure 23 shows only those partial paths which 
are generated from extending the shortest path. Since there 
are obstacles between the last node of the path and the 
goal, the process chooses the partial path (1 15 14) to (10 
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20 14) to (14 20 14) to (20 20 14) for the next iteration of 
extending the shortest path. This partial path is visible 
to the goal so the algorithm as before, performs the 
administrative functions and terminates. 



Table XX - SECOND POLYHEDRAL LISTING 
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Polyhedral World 



The second example using a two polyhedral world has 
the points (7 13 7) and (40 15 14) as the start and goal 
points. Figure 24 shows those paths generated from the 
start. As in the last example, the second polyhedral 
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obstacle is taller than the other two polyhedral obstacles. 
As a result, the vertical paths which extend to this 
obstacle will have only one intersecting point on the first 
obstacle the path intersects. Again, the nodes of the path 
are displayed with an "x" at the intersection point. After 
developing these initial paths, the algorithm iterates 
through the process 4 times before the expansion point is 
visible with the goal. Figures 25, 26, 27 and 28 depict the 
process the algorithm used to find the goal. 




Figure 24 - Initial Paths for 
Two Polyhedral World (Example 
2) 
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V. RECOMMENDATIONS AND CONCLUSIONS 



The algorithm in which the source code is based on does 
not end up finding the shortest path from a start to goal, 
but a best path for the two points for a point traversing 
the environment. The concept of three-dimensional path 
planning was considerably more difficult then I imagined. 

As a result, I was not able to implement in the source code 
the portion of the algorithm which solves the last two types 
of paths presented in Chapter IV. However, if implemented, 
the shortest path can be found. 

I initially started work on solving/finding the type of 
path which lies on the same plane as the start and goal 
points . In solving each of the other 3 remaining types of 
paths which this thesis addresses, I found myself developing 
again how I solved the previously solved types of paths . 

Some changes were as simple as changing the number of 
parameters passed into the function or the structure used in 
passing the return value back from the called function to 
the calling function. Other changes included major 
modifications . 

On area which of source code which took many iterations 
and time in forethought was the header information structure 
for each path. Every time I proceeded in solving the next 
type of path, I discovered I needed more information or in 
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the case of changing a function's structure not needing 
specific datum. 

This algorithm is only the first step in implementing a 
path planner in the NPS II AUV. In addition to including in 
the source code the portion of the algorithm which solves 
the last two types of paths, other modules will need to be 
added . 

One of the most important to me added is a module which 
insures that the path being extended one more node is a safe 
path for an object larger than a point traversing the world. 
Since the NPS II AUV can be thought of as a cylinder 
traveling through the world, the concept of safe path 
planning is not finding the tangential lines to the 
polyhedrons but finding a cylinder which intersects the 
polyhedron at one point. This concept is not as easy as it 
appears. One particular difficult problem will be in 
dealing with polyhedrons in which the sides are not 
perpendicular to the ground. 

Other modules which should be added after the safe path 
module is added concern implementing the coefficient 
characteristics of the AUV in the path planner as well as 
sea and weather conditions and procedures which will 
aggressively limit the size of the world. The order of 
magnitude for this path planner is driven by not only the 
number of polyhedrons, but also the number of nodes in each 
polyhedron. By reducing the search space, time can be saved 



76 



in the planning process . With regards to the conditions and 
coefficients, they not only affect the performance of the 
AUV, but they also affect the determination of a best path 
and need to be included. 
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APPENDIX A 



This appendix contains the source code which is in the file main.c 
#def ine MAIN 



#include "3d_tan.h" 
# include "plot.h" 



/* 



void main() 

{ 



MAIN 

-0O0 



int 

equation 

paths 

a_path 

decision ( ) ; 



choice, iteration_count = 0; 
plane_equation ; 

*current_path = NULL; 
*goal_path = NULL; ; 



polyhedron_list = create_obstacle_list ( ) ; 

boundry = ass ign_boundry_points ( ) ; 
printf ( "\nSTART POINT"); 

start_point - input_point ( start_point ) ; 

printf ( "\nGOAL" ) ; 

goal - input_point ( goal ) ; 



V 



inside_boundries (choice, boundry, start_point, goal); 
goal_direction = atan2(goal.y - start_point . y , 
goal.x - start_point . x) ; 



/* CHECK VISIBILITY BETWEEN START AND GOAL */ 
if ( ! intersect_polyhedron( start_point, goal)) { 

printf ("The best path to follow is from the start to goal"); 
exit (1) ; 

} 

/* FIND PARTIAL PATHS FROM THE START TO 1ST SET OF TANGENTS */ 
start_path = start_f inding_paths ( start_path, start_2d, start_point, goal, 

boundry, head_active_nodes ) ; 
print_each_path( start_path, ++iteration_count ) ; 

# if def DOS 
Pause( ) ; 

#endif 

/* FINDS THE BEST PATH FROM THE INITIAL PARTIAL PATH IN THE PATH LIST */ 

while ((goal_path = visibility ( f ind_shortest_path(start_path) , 

goal, start_point , start_2d)) == NULL) { 

if(l != iteration_count ) 

pr int_each_path ( start_path, iteration_count++ ) ; 
else 

iteration_count++ ; 

print_path_shortest ( shortest_path) ; 

shortest_path = extend_path( start_point , goal, shortest_path, start_2d, 

boundry, head_active_nodes ) ; 

} 

fprintf ( fpt2, "\n\nTHE GOAL HAS BEEN REACHED"); 
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print_each_path( start_path, ++iteration_count ) ; 
print_path_shortest ( shortest_path) ; 

shortest_path = goal_reached_update ( goal / shortest_path , goal_path); 

# ifdef DOS 

if ( plot2screen ) 

draw_shortest_path(shortest_path) ; 

#endif 

print_each_path( start_path, ++iteration_count ) ; 
pr int_path_shortest ( shortest_path ) ; 

# ifdef DOS 

if (plot2screen) { 
closegraph( ) ; 

} 

#endif 

fclose( f pt2 ) ; 

} 

/* 

INPUT POINT 

oOo 

This function allows the user to input a pt into a structure (record) 
and returns that structure to the calling function 

V 

point 

input_point ( pt ) 

point pt; 

{ 

double x_coord, y_coord, z_coord; 

printf ( "\nlnput x, y, z coordinate: H ); 

scanf{"%lf %lf %lf", POINT_VALUES ) ; /* macro to assign values to x,y,z */ 

pt.x = x_coord, pt.y = y_coord, pt.z = z_coord; 
return ( pt ) ; 



/* - - 

DECISION 

0 Q 0 

*/ 

int decision ( ) 

{ 

int choice = 0; 

char answer; 

do { 

printf ( "\nCHOOSE ONE\nl . IN POOL l\n2 . IN POOL 2\n3 . IN POOL 3\n4 . IN POOL 

4\n " ) ; 

printf ("5. IN POOL 5\n"); 
scanf("%d", &choice) ; 

/* ESTABLISH THE ENVIRONMENT */ 



switch(choice) { 
case 1: 

filename = 
break ; 
case 2 : 

filename = 
break; 
case 3: 

filename = 
break ; 
case 4 : 

filename = 



"pool . dat " ; 



"pooll . dat " ; 



"pool 3 . dat " ; 



"pool4 . dat" ; 
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break ; 
case 5 : 

filename = ”pool2 . dat " ; 
break; 
default : 

printf ( "\n\nINCORRECT ENTRY. Try again"); 
clrscr ( ) ; 

] 

] while( ! choice) ; 

printf ("Do you want to print results to a file (y or n)? "); 
fflush( stdin ) ; 
answer = getc(stdin); 

if (answer == 'y' | | answer == 'Y') { 

print2file = 1; 

printf ( "\nThe file name is path . doc . \n" ) ; 

} 

else 

print2file = 0; 
if (print2file) { 

if ( ( f pt2 = f open ("path.doc", WRITEONLY)) == NULL) ( 

perror(" Error: Data file for wrinting did not open correctly. \n "); 

exit ( 0 ) ; 

] 

) 

# ifdef DOS 

printf ( "\nDo you want a plot of this path on the screen (y or n)?"); 
f flush ( stdin ) ; 
answer = getc(stdin); 
if (answer == 'y' | | answer == 'Y') 

plot2screen = 1; 
else 

plot2screen = 0; 

#endif 

printf ( "\nDo you want a horizontal path only (y or n)? "); 

f flush ( stdin ) ; 
answer - getc(stdin); 
if ( ' y ' == answer) 
horizontal = 1; 
else 

horizontal - 0; 



) 
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/ 



INSIDE BOUNDRY 

o 0o 

This function checks whether or not the start point and goal point/points 
are within the boundries of the pool. It calls boundary check passing to 
it the point to be checked.*/ 



void 

int 

boundries 

point 



inside_boundries (choice, boundry, start_point, goal) 
choice; 
boundry ; 

start_point, goal; 



{ 

int 



boolean = FALSE; 



if (choice && (! boundry_check (boundry , start_point ) ) ) { 

printf ( "\nThe start point is not within the boundaries\n" ) ; 
boolean = TRUE; 

} 

if (choice && (! boundry_check ( boundry , goal))) [ 

printf ( "\nThe goal is not within the boundaries\n " ) ; 
boolean = TRUE; 

} 

if (boolean) 
exit ( 1 ) ; 

} 



/* 



BOUNDRY CHECK 

oOo 

This function determines if the passed in point lies within the 
of the search space. If it is not, a FALSE (0) is return, else 
is returned to the calling function.*/ 



boundries 
a TRUE (1) 



int boundry_check (boundry , pt) 

boundries boundry; 

point pt; 

{ 

if (pt.x > boundry. xl && pt.x < boundry. x2 && 
pt.y > boundry. yl && pt.y < boundry. y2 && 
pt.z >= boundry. zl && pt . z < boundry. z2) 
return (TRUE); 
else 

return (FALSE); 
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/* 



ASSIGN BOUNDRY POINTS 

0O0 

This function assigns the boundry points to the search area. At present 
it is fixed at the pool dimensions used in the pool.dat; however, at a 
later date, input function can be encorporated to allow the user to speci- 
fy the search area. 

V 



boundr ies 

assign_boundry_points ( ) 

[ 

boundries boundry; 



boundry. xl = 0; 
boundry. x2 = 4 8; 
boundry. yl = 0; 
boundry. y 2 = 34; 
boundry. zl = 0; 
boundry. z2 = 15; 



return (boundry); 

} 



/* 



FIND SHORTEST PATH 

oOo 

This function finds the shortest path and assigns the global pointer to 
it*/ 



point 

f ind_shortest_path( start_path) 
paths *start_path; 

( 

/* next for structure is used only to find shortest path */ 
paths *current_path; 

point pt; 



shortest_path = NULL; 

for (current_path = start_path, shortest_path = current_path; 
current_path 1= NULL; 

current_path = current_path->next_path) [ 

if ( (current_path-> total_cost < shortest_path->total_cost ) 
&& current_path- >able2expand) 
shortest__path = current_path; 
if ( ! shortes t__path->able2expand ) 

shortest_path = shortest_path->next_path; 

} 

if (plot2screen) 

id_shortest_path_for_expansion (shortest_path ) ; 
return ( shortest_path- >las t_node- >pt ) ; 

} 
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APPENDIX B 



((include "3d tan.h" 



/* 



BUILD TWO D POLYGON LIST 

oOo 

This function is the major function which builds the two dimensional environment 
from the 3 dimensional environment. It accepts as a parameterthe perpendicular 
plane (labeled plane) and places into a link list theintersection points of a 
polyhedron with the plane. These intersectionpoints form the polygon. 

V 



polygon_list *build_two_d_polygon_list ( start_2d, plane, null_ptr) 



t 



polygon_list 

equation 

int 



*start_2d; 
plane; 
null_ptr ; 



obstacle_list *current_polyhedron = polyhedron_list ; 

polyhedron_obstacle *current_f ace; 

polyhedron_obstacle_plane *current_node; 

polygon_list *polygon, *last_polygon , *previous; 

int count, intersection_count , boolean = FALSE; 

static int polygon_number = 1; 

point ptl, pt2 ; 



plane_number++ ; 

if ( start_2d != NULL) [ 

for ( last_polygon - start_2d; /*finds last 2d polygon*/ 

last_polygon ->next_polygon != NULL; 

previous = last_polygon , last_polygon = last_polygon->next_polygon ) ; 

} 

for (current_polyhedron = polyhedron_list ; current_polyhedron != NULL; 
current_polyhedron = current_polyhedron- >next_polygon ) ( 

if ( start_2d == NULL) ( 

NEW_P0LYG0N(start_2d) ; 
last_polygon = start_2d; 
plane_number = 1; 

} 

else { 

if (null_ptr) { /* create a new node if the last node is not null */ 
NEW_POLYGON (last_polygon->next_polygon) ; 
previous = last_polygon ; 

last_polygon = last_polygon- >next_polygon ; 

} 

} 

last_polygon->polygon_start = NULL; 
last_polygon->next_polygon = NULL; 
last_polygon->plane_number = plane_number ; 
last_polygon->polygon_number = polygon_number++; 

last_polygon->polyhedron_number = current_polyhedron- >polyhedron_number ; 

for (current^face = current_polyhedron->obstacle, intersection_count = 0; 
current_face != NULL; 

current_face = current_face->next_face) { 

for (current_node = current_face->face_nodes , count = 1, 
boolean = FALSE; 

count <= current face- >face_nodes- >number_nodes ; 
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current_node = current_node- >ccw, count++) { 
if (boolean == FALSE) { 

ptl = intersection_point ( current_node->pt , current_node- >ccw->pt , 

plane) ; 

if (ptl.x ! = NO_INTERSECTION) { 
boolean = TRUE; 
intersection_count++ ; 
break; 

] 

} 

} 

if (boolean) { 

last_polygon->polygon_start = create_list ( last_polygon->polygon_start , 

intersection_count, ptl); 

} 

) 

if ( last_polygon- >polygon_start != NULL) { 
null_ptr = TRUE; 

last_polygon- >polygon_start->number_nodes = 

count_polygon_nodes ( last_polygon ) ; 
last_polygon = remove_dupl icate_nodes ( last_polygon ) ; 
last_polygon = connect_l inks ( last_polygon ) ; 

} 

else 

null_ptr = FALSE; 



} 

if ( last_polygon->polygon_start == NULL) 
previous- >next_polygon = NULL; 
return (start_2d); 

} 

/* 

CREATE LIST 

0O0 

This function accepts the polygon_start, the intersection_count , and the ptl 
parameters and places the point (ptl) into the link list for 2d polygons 
( polygon_s tart ) . 

V 

obstacle_plane *create_l ist ( polygon_start , intersect ioncount, ptl) 
obstacle_plane *polygon_start ; 
int intersectioncount; 

point ptl; 

{ 

obstacle_plane *temp_ptr, ^current, ^previous; 
int count; 

obstacle_plane *two_d_ptr; 

if ( polygon_start == NULL) { 

NEW_PLANE(polygon_start ) ; 

polygon_start- >pt = assign_point_values (ptl ) ; 
polygon_start->number_nodes = intersection_count ; 
polygon_start- >ccw - polygon__s tart->cw = NULL; 

) 

else { 

NEW_PLANE(temp_ptr) ; 

temp_ptr->pt = assign_point_values ( ptl ) ; 
temp_ptr->ccw = NULL; 

for (previous = polygon_start , current = polygon_start , count = 1; 
current != NULL; 

previous = current, current = current->ccw, count++) { 

if ( current- >pt . x == ptl.x && current->pt . y == ptl.y && 
current- >pt . z == ptl.z) { 
intersection_count -*= 1; 



86 



polygon_start->number_nodes = intersection_count ; 

free ( teinp_ptr ) ; 

return (polygon_start ) ; 

] 

) 

previous ->ccw = temp_ptr; 

polygon_start- >number_nodes = intersection_count ; 

) 

return ( polygon_s tart ) ; 



/* 



This function assigns 
a point structure 

V 



ASSIGN POINT VALUES 

0 Q 0 

the coordinate values (x, y, 



z) 



to ptl which is declared as 



point 

point 

{ 

point 



assign_point_values (pt2 ) 
pt2 ; 

ptl; 



ptl . x = pt2.x; 
ptl.y = pt2 . y ; 
ptl . z = pt2.z; 
return ( ptl ) ; 



/* - — 

CONNECT LINKS 

0O0- - 

This function connects the links of the polygon_list so that each polygon is can 
be accessed using a doublely linked list 

V 

polygon_list *connect_links (polygon ) 
polygon_list *polygon; 

{ 

obstacle_plane *current = polygon->polygon_start ; 
int count; 

for (; current->ccw ! = polygon- >polygon_start; current = current->ccw) 
current- >ccw->cw = current; 

polygon->polygon_start->cw = current; 
return (polygon); 



/* 



COUNT POLYGON NODES 

0O0 

This function counts the number of nodes that make up the polygon and places the 
value in the the polygon's start node structure 
*/ 



int count_polygon_nodes (polygon ) 

polygon_list *polygon; 

{ 

obstacle_plane *current = polygon- >polygon_start- >ccw; 
int count = 1; 



for (; current != polygon->polygon_start ; 
current = current- >ccw, count++) 
if ( current->ccw =- NULL) 
current- >ccw = polygon->polygon_start ; 
return (count); 

) 
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/* 



REMOVE DUPLICATE NODES 

Oo 

This function removes any duplicate nodes which are placed in the polygon link 
listed when created 

V 



polygon_list *remove_duplicate_nodes ( polygon ) 

polygon_list *polygon; 

{ 

obstacle_plane *start = polygon- >polygon_start , 

♦current = start->ccw, *previous = start; 
int countl, number_duplicate_nodes = 0; 



for (countl = 1; 

countl <~ start- >number_nodes ; 

countl++, previous = current, current = current- >ccw) { 

if (current- >pt . x == previous- >pt . x && 
current- >pt . y == previous->pt . y && 
current- >pt . z == previous- >pt . z ) { 
number_duplicate_nodes++ ; 

if (start->pt.x == current->pt . x && 
start->pt.y == current ->pt . y && 
start->pt.z == current->pt . z ) { 
polygon- >polygon_start = previous; 
previous - >number_nodes = current- >number_nodes ; 

} 

previous ->ccw = current- >ccw; 
free((char *) current); 
current = previous- >ccw; 

} 

} 

polygon ->polygon_s tart- >number_nodes -= number_duplicate_nodes ; 
return (polygon); 



/* 

REMOVE PLANES 

0O0 - 

This function deallocates the memory associated with the polygons formed by the 
intersection of the perpendicular plane and the polyhedron 

V 

polygon_list *remove_p lanes ( start_2d) 
polygon_list *start_2d; 

{ 

polygon_list *previous = start_2d, *current = start_2d- >next_polygon ; 
obstacle_plane *nodel, *node2; 

for(; current->next_polygon != NULL; 

previous = current, current = current- >next_polygon ) { 

for(nodel = previous ->polygon_start, node2 = nodel->ccw; 
node2 != previous- >polygon_start ; 
nodel = node2, node2 = node2->ccw) 
f ree( nodel ) ; 
free (nodel) ; 

free ( previous ) ; 

} 

return (NULL) ; 
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APPENDIX C 



#include "3d tan.h" 



/* 



CREATE OBSTACLE LIST 

0O0 

This function is the main function used to create the 3D obstacle environ-ment . The 
list of obstacles use pointers with the main list made up of major obstacles only. 
Off each record in this list a pointer points to the first face of the polygon, 
which in turn points at the next one. This continues until a list of faces is 
established. With in each face record is a pointer which points at a linked list 
of nodes. It calls create_face passing to it the pointer within the polygon record 
that points to the first face of the polygon. It also passes the file pointer to 
the data file*/ 



obstacle_list *create_obstacle_list ( void ) 

{ 

FILE * f pt ; 

int count; 

obstacle_list *polyhedron_list , *polygon = NULL; 

/* opens file if one, else exits program on an error */ 
if ((fpt = fopen( filename, READONLY)) == NULL) { 

perror(" Error: Data file did not open correctly. \n " ) ; exit(l); 

} 

else ( 

read_comment ( fpt ) ; 

fscanf(fpt, "%d", &number_polygons ) ; 

for (count = 1; count <= number_polygons; count++) { 
if (count == 1) { 

NEW_NODE( polyhedron_list ) ; 
polygon = polyhedron_l ist ; 

) 

else { 

NEW_NODE( polygon ->next_polygon ) ; 
polygon = polygon - >next_polygon ; 

} 

polygon->polyhedron_n umber = count; 

polygon->obstacle = create_f ace( polygon->obstacle, fpt); 
polygon- >next_polygon = NULL; 

} 

} 

return ( polyhedron_list ) ; 



/* 



CREATE FACE 



O 0o 

This function creates the list off the major polygon list and makes up a list of 
polygon faces. It call create_plane, passing it the file pointer to the data file 
and the pointer within each face that will point to the first node of the linked 
node list. It returns a the start record (first face in the polygon list) to the 
calling function*/ 



polyhedron_obstacle *create_f ace( start_face, fpt) 
polyhedron_obstacle *start_face; 

FILE * fpt; 

{ 

int number_f aces, count; 

polyhedron_obstacle *face; 
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fscanf(fpt, "%d", &number_faces ) ; 

for (count - 1; count <= number_f aces ; count++) { 
if (count == 1) [ 

NEW_POLYHEDRON ( star t_f ace ) ; 
face = start_face; 

} 

else { 

NEW_POLYHEDRON(face->next_face) ; 
face = face->next_face; 

} 

face->next_face = NULL; 

face-> face_nodes = create_plane( fpt, face->face_nodes ) ; 

} 

return ( s tart_f ace ) ; 



/* 



CREATE PLANE 

0O0 

This function creates the floatly linked list made up of the nodes of the face and 
returns the start of the list to the calling function.*/ 



polyhedron_obstacle_plane *create_plane( f pt , start_plane) 
polyhedron_obstacle_plane *start_plane; 

FILE * fpt ; 

( 

polyhedron_obstacle_plane ^current = start_plane, ^previous = NULL; 
int count = 1, nodes; 

double x, y, z; 

previous = NULL; 
fscanf(fpt, "%d" , &nodes); 

for (count = 1; count <= nodes; count++) [ 
fscanf(fpt, M %lf %lf %lf", &x, &y , &z); 
if (count == 1) { 

NEW_POLY_PLANE(start_plane) ; 
previous = start_plane; 
current = start_plane; 

] 

else { 

NEW_POLY_PLANE ( current- >ccw) ; 
current = current->ccw; 

) 

current->pt . x = x; 
current->pt . y = y; 
current->pt . z = z; 
current- >cw = previous; 
current->ccw = NULL; 
previous = current; 

} 

s tart_plane->cw = current; 
current->ccw = start_plane; 
s tart_plane->number_nodes = nodes; 
return ( s tart_plane) ; 



/* 



READ COMMENT 

oOo 

This function is used only to strip the comments off the polygon data file so that 
they do not corrupt the link list, but an explanation of the file makeup remains 
with the data file 

V 



void read_comment ( fpt ) 

FILE *fpt; 

[ 
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char 



word [ 20 ] ; 



do 

fscanf(fpt / "%s", word); 
while (word{2] != 



/* 

DISTANCE 

o 0o 

This function calculates the distance from one point to another and re-turns it to 
the calling function*/ 



double 

point 

[ 

double 



distance( ptl , pt2) 
ptl, pt2 ; 

a , b, c, z ; 



a = ptl.x - pt2 . x ; 

a = sqr(a) ; 

b = ptl.y - pt2 . y ; 

b = sqr(b) ; 

c = ptl.z - pt2.z; 

c = sqr ( c) ; 

z = sqrt(a + b + c); 



return ( z ) ; 

} 



/* 



FIND PLANE EQUATION 

o 0o 

This function finds a plane give three points. It finds two vectors formed by 
ptlpt2 and ptlpt3, then calls cross product to find the a, b and c coefficents, and 
finally solves for d using a,b,c and ptl */ 



equation 

point 

{ 

point 

equation 



f ind_plane_equation (ptl, pt2, pt3) 
ptl, pt2 , pt3; 

vectorl_2, vectorl_3; 
pl_eq; 



vectorl_2.x = 
vectorl_2.y = 
vectorl_2.z = 
vectorl_3.x = 
vectorl_3.y = 
vectorl 3 . z = 



pt2.x - ptl.x; 
Pt2 . y - ptl.y; 
pt2 . z - ptl.z; 
pt3 . x - ptl.x; 
Pt3 . y - ptl.y; 
pt3.z - ptl.z; 



pl_eq = cross_product ( vectorl_2 , vectorl_3); 

pl_eq.d = ( ( pl_eq . x * (-1.0) * ptl.x) + (pl_eq.y * (-1.0) * ptl.y)); 
pl_eq.d = pl_eq.d + (pl_eq.z * (-1.0) * ptl.z); 
return (pl_eq); 



/* 



CROSS PRODUCT 

oOo 

This function finds the cross product of two vectors and returns the coefficients 
to the calling function.*/ 



equation 

point 

{ 

equation 



cross_product (vectorl , vector2) 
vectorl, vector 2; 

plane; 



plane. x = (vectorl. y * vector2.z) - (vectorl. z * vector2.y); 

plane. y = -1.0 * (( vectorl. x * vector2.z) - (vectorl. z * vector2.x)); 
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plane. z = (vectorl.x * vector2.y) - (vectorl.y * vector2.x); 
return ( plane ) ; 



} 



/* 



INTERSECT POLYHEDRON 



0O0 

This function determines if the line formed from two points intersect a polyhedron. 
If so, it returns 1, else it returns zero. It calls f ind_plane_equation, passing 
it three nodes on the current face being checked and receives the plane equation. 
It then calls inter-section_point to determine if the line intersects the plane. 
This function returns the point if there is an intersection, else it sets the x 
coeffi-cient = -999999. If there is an intersection point, the function call 
lines_intersect, which determines if the intersection is within the boun-dries of 
the plane. 

V 



int 

point 



intersect_polyhedron (ptl , pt2) 
ptl, pt2 ; 



obstacle_l ist *current_obstacle; 
polyhedron_obstacle *face; 
polyhedron_obstacle_plane *node; 
equation pl_eq; 

for (current_obstacle = polyhedron_list ; current_obstacle != NULL; 
current_obstacle = current_obstacle- >next_polygon ) { 

for (face = current_obstacle->obstacle; face != NULL; 
face = face- >next_f ace) { 

node = face->face_nodes ; 

pl_eq = f ind_plane_equation( node->pt, node- >ccw->pt , 

node->ccw- >ccw->pt ) ; 

if ((pl_eq.x * ptl.x + pl_eq.y * ptl . y + pl_eq.z * ptl . z + pl__eq.d) == 0 

( pl_eq . x * pt2.x + pl_eq.y * pt2.y + pl_eq . z * pt2 . z + pl_eq.d) == 0) 
continue; /* if ptl lies on the plane, go to next plane */ 

intersection = intersection_point ( ptl , pt2, pl_eq); 

if ( intersection .x != NO_INTERSECTION ) { 

if (! lines_intersect ( intersection, f ace->f ace_nodes , pleq)) 
return ( 1) ; 

) 

} 

} 

return (0); 



/* 



INTERSECTION POINT 

Q0Q 

This function finds the intersection of a plane and a line. It accepts three 
arguments, 2 points (the line) and an equation for the plane. It returns the 
intersection point if there is one or it sets the plane. x value equal to -99999.0. 
This value is a flag that lets the calling function know that there is no 
intersection point and the line and plane are parallel.*/ 



point 

point 

equation 

{ 

double 



intersection_point (ptl, pt2, plane) 
ptl, pt2; 
plane ; 

y * 0.0, t = 0.0; 
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y = (plane. x * (pt2.x - ptl.x) + plane. y * (pt2.y - ptl.y)); 
y = y + (plane. z * (pt2.z - ptl.z)); 
if (y ~ FALSE) { 

intersection . x = NO_INTERSECTION; 
return (intersection); 

} 

t = (plane. x * ptl.x + plane. y * ptl.y + plane. z * ptl.z + plane. 6); 
t = -1 * t / y; 

if (0.0 <= t && t <= 1.0) { 

intersection . x = ptl.x + (pt2.x - ptl.x) * t; 

intersection . y - ptl.y + (pt2.y - ptl.y) * t; 

intersection . z = ptl.z + (pt2.z - ptl.z) * t; 

} 

else 

intersection . x = NO_INTERSECTION ; 
return (intersection); 

} 

/*- 

LINES INTERSECT for INSIDE POLYGON CHECK 

o 0o 

This function determines if a point is within the boundries of a polygon. If the 
point is, the function returns 1, else it returns 0. Since the plane can be on any 
plane in the 3d environment, the function must check each plane face (xy, xz, yz) . 

V 



int lines_intersect( intersect , start, pl_eq) 

point intersect; 

polyhedron_obstacle_plane *start; 
equation pl_eq; 

{ 

polyhedron_obstacle_plane ‘current = start; 

int number_nodes = start- >number_nodes , count; 

int intersect_count - 0; 

double t, s, b, c; 

point temp_pt; 

temp_pt = find_point_on_obstacle_plane_face( intersect , pl_eq); 

for (count = 1; count <= number_nodes ; count++, current = current- >ccw) { 

/* CHECK X Y PLANE */ 

b = (temp_pt.x - intersect. x) * (current- >ccw->pt . y - current- >pt . y ) ; 
c = (temp_pt.y - intersect. y) * (current- >ccw->pt . x - current- >pt . x ) ; 
b = b - c; 

if (b 1= 0) ( 

s = (intersect. y - current->pt . y ) * ( current->ccw- >pt . x - current- >pt . x) ; 
s -= (intersect. x - current->pt . x) * (current- >ccw->pt . y - current->pt . y) ; 
s - s / b; 

t = (intersect. y - current->pt . y ) * (temp_pt.x - intersect . x ) ; 
t = t - (intersect. x - current- >pt . x ) * (temp_pt.y - intersect . y) ; 
t - t / b; 

if (0 <= s && s <= 1 && 0 <= t && t <= 1) { 
intersect_count++ ; 
continue; 

] 

] 

/* CHECK X Z PLANE */ 

b = (temp_pt.y - intersect. y) * ( current- >ccw->pt . z - current->pt . z ) ; 
c = (temp_pt.z - intersect. z) * ( current- >ccw->pt . y - current->pt . y ) ; 
b = b - c; 
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if (b ! = 0) { 

s = (intersect. z - current- >pt . z ) * ( current- >ccw- >pt . y - current->pt . y ) ; 
s -= (intersect. y - current->pt . y ) * ( current->ccw- >pt . z - current->pt . z) ; 
s - s / b ; 

t = ( intersect. z - current- >pt . z ) * (temp_pt.y - intersect . y ) ; 
t = t - (intersect. y - current- >pt . y ) * (temp_pt.z - intersect . z ) ; 
t « t / b; 

if (0 <= s && s <= 1 && 0 <= t && t <= 1) ( 

intersect_count++ ; 
continue; 

) 

} 

/* CHECK Y Z PLANE */ 

b = (temp_pt.x - intersect. x) * (current->ccw->pt . z - current- >pt . z ) ; 
c = (temp_pt.z - intersect. z) * ( current->ccw->pt . x - current- >pt . x ) ; 

if (b - c >= le-3) { 

s = (intersect. z - current->pt . z ) * ( current->ccw- >pt . x - current- >pt . x ) ; 
s -= (intersect. x - current->pt . x ) * ( current->ccw- >pt . z - current->pt . z) ; 
s = s / (b - c) ; 

t = (intersect. z - current- >pt . z ) * (temp_pt.x - intersect . x ) ; 
t = t - (intersect. x - current- >pt . x ) * (temp_pt.z - intersect . z ) ; 
t = t / (b - c); 

if (0 <= s && s <= 1 && 0 <= t && t <= 1) ( 

intersect_count++ ; 
continue ; 

} 

] 

} 

if ( intersect_count % 2 == 0 ) 
return (1); 
else 

return ( 0 ) ; 



/* 



FIND POINT ON OBSTACLE PLANE FACE 



0 Q q 

This function finds a point that is on the plane. It is called by the function 
that checks if the intersection of the ray formed by two points intersects any 
polyhedrons in the environment.*/ 



point 

point 

equation 

{ 

point 



f ind_point_on_obstacle_plane_face( intersect, plane) 
intersect ; 
plane; 

Pt; 



pt . z - -5 ; 

if ( (plane. x == 0 | | plane. y == 0) && plane. z == 0) { 
pt.x = intersect. x; 
pt.y = intersect. y; 

) 

else { 

pt.y = 17; 
if (plane. x != 0) 

pt.x = -(plane. d + plane. z * pt . z + plane. y * pt.y) / plane. x; 
else 

pt.x = 50; 

) 

return (pt); 
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/* 



This function 

V 



VERTICAL AND PERPENDICULAR PLANES 

o 0o 

finds the vertical and perpendicular planes formed by two points. 



void 

point 



vertical_n_perpendicular_plane( ptl , pt2 ) 
ptl, pt2 ; 



point pt3; 



pt3 . x = pt2 . x; 
pt3.y = pt2 . y ; 
pt3 . z = pt2 . z + 1; 

vertical = f ind_plane_equat ion (ptl, pt2, pt3); 
if (ptl.x !- pt2.x) [ 
pt3.x - pt2 . x ; 
pt3 . y = pt2 . y + 1; 

] 

else { 

pt3.x = pt2.x + 1; 

Pt3 . y = pt2 . y ; 

] 

pt3 . z = pt2.z; 

perpendicular = f ind_plane_equation ( ptl , pt2, pt3); 
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APPENDIX D 



#include "3d tan.h" 



/* 



CHECK ACTIVE NODE LIST 
OOO 



This function checks the active node list to determine if the expanded 
node found in f ind_tangents_f rom_inner_nodes has already been used as a 
node to an existing path. If the point has been used, the function deter- 
mines if the possible new path has a cost less than the existing path. If 
the cost is less, the function calls the function mark_old_path_with_node_ 
unexpandable, which marks the path as unexpandable . If the new possible 
path has a smaller cost, then the function returns a one (1) else it 
returns a zero (0). 

V 



int 

active_nodes 

paths 

point 

{ 

active_nodes 

a_path 



check_active_node_list ( head_active_nodes , 
*head_active_nodes ; 

♦shortest; 

Pt; 

♦current = head_active_nodes ; 

♦last_node = shortest- >last_node; 



shortest , 



Pt) 



for (; current ! = NULL; current = current->next_act ive_node) [ 

if (current- >pt . x == pt.x && current- >pt . y == pt . y && current- >pt . z == pt.z) 

{ 

if ( current->distance > ( shortest->cost + 

distance(shortest->last_node->pt , pt))) { 
start_path = 

mark_old_pa th_wi th_node_unexpandable ( current - >when_path_f ound ) ; 
return ( 1 ) ; 

] 

else 

return(O) ; 

} 

} 

return ( 1 ) ; 

] 



/* 

MARK OLD PATH WITH NODE UNEXPANDABLE 

- OOO- - - 

This functions is called if the existing path has been determined as too 
expensive (ie; there is a node in the path that can be reached by another 
path with a smaller cost). The function searches the path list to find 
the correct path and then marks the flag "able2expand " as FALSE (0). 

V 

paths *mark_old_path_with_node__unexpandable( path_number ) 

int pa th_n umber; 

t 

paths *current = start_path; 

for (; current != NULL; current = current- >next_path) { 
if ( current- >when_path_found == path_number) 
current ->able2expand * FALSE; 

} 

return ( start_path ) ; 
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} 



/*--- 

INSERT NODES INTO ACTIVE NODE LIST 

OOO -*/ 

active_nodes * insert_nodes_into_active_node_list ( head_node_lis t , pt , 

cost) 

active_nodes *head_node_lis t ; 

point pt; 

double cost; 

{ 

static active_nodes * last_node_added = NULL; 

if ( head_node_list == NULL) [ 

NEW_ACTIVE_NODE(head_node_list) ; 
last_node_added = head_node_list ; 
last_node_added- >next_active_node = NULL; 

) 

else { 

NEW_ACTIVE_NODE ( last_node_added- >next_active_node) ; 
last_node_added = last_node_added->next_active_node; 
last_node_added->next_active_node - NULL; 

) 

last_node_added->pt = ass ign_point_values ( pt ) ; 
last_node_added~>distance = cost; 

last_node_added->when_path_found = number_paths_found; 
return ( head_node_lis t ) ; 

} 
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APPENDIX E 



This appendix contains the source code I began to implement for going up, around 
and up type of paths as described in Chapter IV. The file over&@.c contains the 
source code developed todate and not yet implemented 

^include M 3d_tan.h M 
^include "plot.h" 

#def ine OVER 0 

paths *up_around_up(path, tan, start, goal) 
paths *path; 
int tan; 

{/* 

polyhedron_f ace *from_face = NULL, *to_face = NULL; 
obstacle_list *polyhedronl , *polyhedron2 ; 
a_path *tangent_list ; 
vertical_path *vertical_list; 
int count; 

polygon_list *polygons, *recursive_polygons = NULL; 
paths *partial_path = NULL, ^shortest, *short_list; 

for (polyhedronl = polyhedron_list ; 
polyhedronl != NULL; 

polyhedronl = polyhedronl->next_polygon ) { 
from_face = find_polyhedron_f ace ( polyhedronl ) ; 

for (polyhedron2 = polyhedron_list ; 
polyhedron2 1= NULL; 

polyhedron2 = polyhedron2- >next_polygon ) { 

if ( polyhedron2 ->polyhedron_number ~ polyhedronl - >polyhedron_number | 
polyhedron2 ->polyhedron_number == path- >polygon4vert ) 
continue; 
else 

to_face = f ind_polyhedron_face(polyhedron2 ) ; 

/*since tangent returns a tangent between two polygons, have to form a 
polygon list with only the to_face and from face in the list so that a 
the tangent is found*/ 

polygons = make_l ist ( f rom_f ace, to_face); 
for (count = 1; count <= 4; count++){ 

if ( NULL == ( tangent_list = tangent ( path, from_face, to_face->face_nodes , 
(count -=1 | 3?MINUS : PLUS ) , polygons, OVER, 
count ==1 | 27MINUS : PLUS ) ) ) printf("you blew it caddell " ) ; 

if ( ! chech_tangent_list ( tangent_list , path)) continue; 

if ( 1 intersect_polyhedron ( tangent_list- >pt , tangent_list->next_node->pt ) ) 
continue; 
else{ 

vertical_l ist = f ind_vertical_nodes ( tangent_l ist- >next_node- >pt , 

tangent_list->pt) ; 

vertical_n_perpendicular_plane (vert ical_l ist- >next_node->next_node->pt , 
path->last_node->pt) ; 

recurs ive_polygons = build_two_d_polygon_list ( recurs ive_polygons , 

perpendicular, FALSE); 

partial_paths = duplicate_path( path) ; 
partial_paths->plane_number = plane_number ; 
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partial_paths = cont inue_vertical_path ( partial_paths , recurs ive_polygons 



recurs ive_polygons = f ree_polygon_list ( recursive_polygons ) ; 

} 

}/*end count for structure*/ 

] /*end inner for structure*/ 

] /*end outer for structure*/ 

free_pseudo_polygon_list( polygons ) ; 
f ree_tangent_list ( tangent_list) ; 

V 

return (path) ; 



polyhedron_f ace * f ind_polyhedron_face(polyhedron ) 
obstacle_list *polyhedron; 

{ 

polyhedron__f ace *face, *min_vert_f ace = NULL; 
obstacle_plane *node, *min_vert_node = NULL; 

for(face = polyhedron- >obstacle; 
face ! — NULL; 
face = face->next_face) { 

node = face- >face_nodes ; 

if ( node->pt . z != node->ccw->pt . z && 

node->pt.z != node->cw->pt . z ) continue; 
else( 

if (NULL == min_vert_face) min_vert_f ace = face; 
else { 

if (min_vert_face->face_nodes->pt . z < node->pt.z) break; 
else min_vert_f ace = face; 

} 

} 

) 

return ( mi n_vert_f ace) ; 

} 



polygon_list *make_list ( to, from) 
obstacle_plane *to, *from; 

t 

polygon_list *polygons; 

NEW_POLYGON ( polygons ) ; 

NEW_POLYGON ( polygons -> next_polygon ) ; 

polygons ->polygon_start - from; 
polygons->next_polygon->polygon_start = to; 
polygons ->next__polygon->next_polygon = NULL; 

return ( polygons ) ; 

] 

int free_pseudo_polygon_list (polygons ) 
polygon_list *polygons; 



l 



free( polygons ->next_polygon ) ; 
free (polygons ) ; 
return ( 1 ) ; 
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int chech_tangent_list ( tangent_list , path) 
a_path * tangent__list ; 
paths *path; 



double dir; 

if (path->forward) dir = goal_direction; 
else dir = norm(goal_direction - HPI); 

if (ABS( norm (dir - 

atan2 ( tangent_list->pt . y - path->last_node->pt . y , 
tangent_list->pt . x - path->last_node->pt . x ) ) ) >=HPI) 
return( 0) ; 

if (abs ( norm(dir - 

atan2 ( tangent_list- >next_node->pt . y - path- >last_node->pt . y , 
tangent_list->next_node->pt .x - path->last_node->pt . x) ) ) >= HPI) 
return ( 0 ) ; 

return(l) ; 

) 

a_path * free_tangent_list ( list ) 
a_path * 1 i s t ; 

{ 

a_path *previous, *current; 

for(current = list, previous = list; 
current != NULL; 

previous = current, current = current- >next_node, free ( previous ) ) 
free ( previous ) ; 
return ( NULL) ; 



polygon_list * free_polygon_list ( list) 
polygon_list *list; 

{ 

polygon_list *previous_poly , *current_poly ; 
obstacle_plane *node; 
int node_number , count; 

for (current_poly = list, previous_poly = list; 

NULL != current_poly ; 

previous_poly = current_poly , current_poly - current_poly- >next_polygon, 
f ree ( previous_poly ) ) { 

for (node = current_poly- >polygon_start , 

node_number - current_poly- >polygon_start- >number_nodes , 
count = 1; 

count <= node_number; 

node = node->ccw, node->cw->ccw = NULL, free ( node->cw) , count++) ; 

} 

free ( previous_poly ) ; 
return (NULL) ; 
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APPENDIX F 



This appendix contains the source code found in path.c 

#include "Sd^tan.h" 

#include "plot.h" 

#def ine LOOPFOREVER 1 
#def ine AROUND PATH 1 



/* 



REMOVE PATHS 



oOo 

This function frees from memory the created possible paths so that another 
iteration of path building can commence. It accepts the head of the list 
and returns a NULL pointer to the head of the list.*/ 



paths 

paths 

{ 

paths 

a_path 

int 



*remove_paths ( start, shortest) 

*start, *shortest; 

*current = start->next_path, *previous = start; 
*node, *nodel; 

i; 



for (previous = start, current = start->next_path; 
current->next_path != NULL; 

previous = current, current = current->next_path) { 

if (shortest_j?ath == previous) { 
shortest->next_path = NULL; 
continue; 

} 

for (node = previous->last_node , nodel = node->next_node; 
nodel ! = NULL; 

node = nodel, nodel = nodel->next_node ) 
free (node) ; 

free (node); /* frees last node in the path being deleted */ 

free (previous ) ; 

} 

if (shortest_path != current) { 
previous = current; 

for (node = previous->last_node, nodel = node->next_node; 
nodel ! = NULL; 

node = nodel, nodel = nodel->next_node) 



/* frees last node in the path being deleted */ 
/* frees last path in the path list */ 



free (node) ; 

free (node) ; 
free (previous) ; 

} 

start = shortest; 
return (start); 
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/ 



ORDER 

oOo 

This function uses the area function for a triangle in determining if the 
current node is the common tangent. It uses one of the two adjacent nodes 
and the inputted point to form the triangle. It returns the value found 
(double) in the calculations.*/ 

double order (pi, current, ad jacent_node) 

point pi; 

obstacle_plane *current, *ad jacent_node; 

{ 

double z; 

z = (current->pt . x - pl.x) * (ad jacent_node->pt . y - pl.y); 
z = z - (ad jacent_node->pt .x - pl.x) * (current->pt . y - pl.y); 
return ( z ) ; 



/* 



START FINDING PATHS 

oOo 

This function finds all the valid tangential lines from the start point 
to polygons in the start_2d list and places them in the start_paths link 
list. It returns this link list to the calling function. The function 
calls plus and minus tangent functions to find the tangents from the start 
to all the polygons in the start_2d list. As it finds the two tangents 
for a polygon, it calls cross_polygon to determine if there is not a poly- 
gon between the point and the start point. If not, then the node is added 
to the list as the beginning of a valid path, by calling create_path_n_nodes . 
It calls assign_point_values, which places the new point into the link list. 
The function also calls the function atan2, which is a library function that 
determines the direction from the start point to the node added to the 
start jpaths list.*/ 



paths 

paths 

polygon_list 
point 
boundries 
active nodes 



*start_f inding_paths (start_paths , start_twod, 
boundry, active_node_list ) 

* start_paths; 

* start_twod; 
start_pt, goal; 
boundry; 

*active_node list; 



start_pt , 



goal , 



{ 



paths 

polygon_list 

obstacle_plane 

int 

point 



*current_path » NULL; 
*current_polygon; 
*current_node ; 
count, tangent; 
ptl, pt2 ; 



plane_number = 1; 



/* tells what plane the 2d representation is 
* on */ 



vertical_n_jperpendicular_plane (start_point , goal) ; 

/*start_2d is a global defining the two d polygon list*/ 

start_2d = build_two_d_polygon_list (start_2d, perpendicular, FALSE); 

#if def DOS 

if (plot2screen) 

data_plot (start 2d); 

#endif 



for (current_polygon = start_2d; 
current_polygon != NULL; 

current ^polygon = current_polygon->next_polygon) { 

ptl = plus_tangent (current_polygon->polygon_start , start_pt); 
pt2 = minus_tangent (current_polygon->polygon_start, start_pt) ; 
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if (direction_heurist ic_f rom_start ( start_pt , ptl) && 
boundry_check (boundry , ptl)) ( 
if ( ! cross_polygon ( start_2d, start_pt, ptl) } ( 

start_paths = create_path_n_nodes ( start_paths, current_path) ; 
tangent = PLUS; 
if (current_path == NULL) 

current_path = start_paths; 
else 

current_path = current_path->next_path; 
current_path = update_current_path (current_path, / * start_paths, */ 

tangent, start_pt, ptl, goal, 
current_polygon->polygon_number ) ; 
current_path = add_start_point_to_jpath (current_path, start_pt); 



#if def 



tendif 



) 

if 



DOS 

if (plot2screen ) 

draw_line_segment (start_pt, current_path->last_node->pt , YELLOW, 
REGULAR) ; 



} 

else ( 

if ( ’horizontal) ( 

start_paths = up_and_over ( start_paths , current_path, start_point, 

ptl, start_point, PLUS, 
current_polygon->polyhedron_number ) ; 
if (current_path == NULL) current_path = start_paths; 
else current_path = current_path->next_path ; 
current_path->forward = YES; 

} 

} 

(direction_heuristic_f rom_start ( start_pt , pt2) && 
boundry_check (boundry, pt2) ) { 

if ( ! cross_polygon (start_2d, start_pt, pt2)){ 

start_paths = create_path_n_nodes ( start_paths , current_path) ; 
tangent = MINUS; 
if (current_path == NULL) 

current_path = start_paths; 
else 

current_path = current_path->next_path; 
current_path = update_current_path ( current_path, tangent, start_pt, 

pt2, goal, 

current_polygon->polygon_number) ; 
current_path = add_start_point_to_path (current_path, start_pt); 



#if de f 



#endif 



} 

} 



DOS 

if (plot2screen) 

draw_line_segment (start_pt, current_path->last_node~>pt , YELLOW, 

REGULAR) ; 



} 

else { 

if ( ! horizontal) { 

start_paths = up_and_over ( start_paths , current_path, start_point, 

pt2 , start_point, MINUS, 
current_polygon->polyhedron_number } ; 
if (current_path == NULL) current_path = start_paths; 
else current_path = current_path->next_path; 
current_path->forward = YES; 



} 



/*this function finds the one partial vertical path from the start 
point over all obstacles to goal*/ 

start_paths = up_and_over ( start_paths, current_path, start_point, goal, 
start_point, VERTICAL) ; 



103 



current_path->next_path->f orward = YES; / *current_path->next is path 

added to the list that is 
up and over all obstacles*/ 
current_path = cur rent_path->next_path; 

/*this function finds all the partial vertical paths from the goal, thus 
working backwards to find all the paths which go around one or more 
obstacles and over at least one obstacles*/ 
start_paths = up_and_over_from_goal (start_paths, current_path, goal, start_2d) 

return ( start_paths) ; 

} 



/* 



ADD START POINT TO PATH 

oOo 

This function adds the start point to the partial list being created by 
the function start finding paths.*/ 



paths 

paths 

point 

{ 

a_path 



*add_start_point_to_path (current_path, start_point) 
*current_path; 
start_point ; 

* current node; 



for (current_node = current_path->last_node ; 
current_node->next_node != NULL; 
current node = current node->next node) ; 



NEW_NODES (current_node->next_node) ; 

current_node->next_node->pt = assign_point_values ( start_point ) ; 
current_node->next_node->next_node = NULL; 
current_node->next_node->next_after_node = current_node ; 
return (current_path) ; 



/* 

CREATE PATH AND NODES 

oOo 

This function is called to create and insert into the start_paths link 
list new points which form valid paths from the start.*/ 

paths *create_path_n_nodes (start_paths , current_path) 

paths *start_paths, *current path; 

{ 

if (start_paths == NULL) { 

NEW_PATH (start_paths) ; 
current_path = start paths; 

) 

else { 

NEW_PATH (current_path->next_path) ; 
current_path = current_path->next_path; 

current_path->polygon4 vert_PTR = NULL; 
current_path->next_path = NULL; 

NEW_NODES ( current_path->last_node) ; 
current_path->last_node->next_node = NULL; 
return ( start_paths) ; 



} 
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/* 



CROSS POLYGON 

oOo 

This function determines if a line crosses the polygon. It checks each 
line segment of the polygon and returns 1 if there is an intersecttion . 
The second for structure is used to check if p2 is on a polygon. If so, 
boolean is asserted to true (int value = 1) . This tells the rest of the 
for loop to go on to the next polygon*/ 



int cross_polygon (start , 

polygon_list *start; 
point pi, p2; 

{ 



int count, boolean; 

polygon_list ^polygon = NULL; 

obstacle_plane ^current = NULL; 
point p3 , p4; 

double w, x, y, z; 



Pi* 



p2) 



for (polygon = start; polygon J = NULL; 

polygon = polygon->next_polygon ) ( 

for (count = 1, current = polygon->polygon_start , boolean = FALSE; 
count <= polygon->polygon_start->number_nodes; 
count++, current = current->ccw) { 
if (p2.x == current->pt.x && p2 . y ~= current->pt . y ) { 

boolean = TRUE; 
break ; 

} 

} 



if (Iboolean) ( 

for (count = 1, current = polygon->polygon_start ; 

count <= polygon->polygon_start->number_nodes ; 
count++, current = current->ccw) { 
p3.x = current->pt . x, p3.y = current->pt . y; 
p4.x = current->ccw->pt . x, p4.y = current->ccw->pt . y ; 
w = sign ( f unction_one (p3 . x, p3.y, pi, p2) ) ; 
x = sign (function_one (p4 . x, p4.y, pi, p2) ) ; 
y = sign ( function_two (pi . x, pl.y, p3, p4)); 
z = sign (function_two (p2 . x, p2.y, p3, p4 ) ) ; 

if (w!=x&&y!=z) { 

return (1); /* line intersects polygon */ 

} 

} 

current = current->ccw; 

} 

} 

return (0 ) ; 



/* 



FUNCTION ONE AND FUNCTION TWO 

oOo 

Funcitons one and two are simple line equaitons used to determine if two 
lines intersect. They both return the value of the equation to the 
calling funcion*/ 



double f unct ion_one (x, y, pi, p2) 



double 


x, y; 


point 


plr p2; 


int 


z = 0; 


z = ( (y - pi .y) 
return (z) ; 


* (p2 . x - pi . x ) ) - ((p2.y - pl.y) * (x - pl.x)); 
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} 



double 

double 

point 

{ 

int 



f unction_two (x, y, p3, p4 ) 
x, y; 
p3, p4; 

z = 0; 



z = ((y - p3.y) * (p4.x - p3.x)) - ((p4.y - p3.y) 

return ( z) ; 



(x - p3 . x) ) ; 



/* 



DUPLICATE PATH 

oOo 

This function accepts from the calling function a valid path, duplicates 
it and inserts the duplication into the start_paths link list. It calls 
duplicate_entries to duplicate the header data of each path (ie type of 
tangent, total cost, estimated cost, direction etc...)*/ 



paths 

paths 

{ 

a_path 

paths 



*duplicate_paths (current) 
*current; 

*node, *duplicate_node; 
*duplicate; 



duplicate = duplicate_entries (current) ; 
for (node = current->last_node ; 
node ! = NULL; 
node = node->next_node) { 
if (duplicate->last_node == NULL) { 

NEW_N0DES (duplicate->last_node) ; 
duplicate_node = duplicate->last_node; 
duplicate_node->next_after_node = NULL; 

} 

else { 

NEW_N0DES (duplicate_node->next_node) ; 

duplicate_node->next_node->next_after_node = duplicate_node; 
duplicate_node = duplicate_node->next_node ; 

} 

duplicate_node->next_node = NULL; 

duplicate_node->polygon_number = node->polygon_number ; 
duplicate_node->pt = assign_point_values (node->pt ) ; 

return (duplicate) ; 



/* 

DUPLICATE ENTRIES 

oOo 

This function duplicates the major entries of a valid path needed in the 
calculations for finding the best path*/ 

paths *duplicate_entries (current) 

paths *current; 

{ 

paths *duplicate; 

NEW_PATH (duplicate) ; 
duplicate->last_node = NULL; 
duplicate->cost = current->cost ; 

duplicate->estimated_cost = current->estimated_cost ; 
duplicate->total_cost = current->total_cost ; 
duplicate->ray_direction = current->ray_direction; 
duplicate->plane_number = current->plane_number ; 
duplicate->type_tangent = current->type_tangent ; 
duplicate->forward = current->forward; 
duplicate->polygon4vert_PTR = NULL; 
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return (duplicate) ; 

} 



/* 



EXTEND PATH 

oOo 

This function accepts three parameters (the start path for either possible 
verticle paths of possible perpendicular paths, the start pointer for the 
2d polygon representation, and the goal point. It searches the possible 
path list for the shortest total distance from start to goal. It checks 
the line projection of this path and the goal with the goal point for any 
intersection with polygons from the polygon list (start_2d list) . If no 
intersections, the goal is appended to this shortest path and the short- 
est path is returned to the the calling funcition. If there is an inter- 
section, the function calls FIND_TANGENTS FROM_INNER_NODES, passing to it 
the shortest path and the start_2d pointer 
*/ 



paths *extend_path (start , goal, shortest_path, 

active node list) 



polygon_list 
point 
paths 
boundries 
active nodes 



* start_2d; 

start, goal; 
*shortest_path; 
boundry; 

^active node list; 



{ 



paths *current_path = start_path; 

a_path *new_record, *current; 



start_2d, boundry, 



if 



(shortest_path->type_tangent 1= NONE) { 

shortest_path = f ind_tangents_f rom_inner_node (shortest_path, 

boundry, active_node_list , 

1 ); 

shortest_path->able2expand = NO; 



start_2d, 



} 

else { 

if (shortest_path->f orward == YES) /^partial path is from start or 

forward*/ 

start_2d = add_on_new_plane_intersection (start_2d, 
shortest_path->last_node , 
goal) ; 

else /*partial path is from the goal or backwards*/ 
start_2d = add_on_new_plane_intersection (start_2d, 
shortest_path->last_node , 
start) ; 

shortest_path->plane_number = plane_number ; 

shortest_path = up_around_up (shortest_path, PLUS, start_point, goal); 

shortest_path = continue_vertical_path (shortest_path, started); 

} 

return (shortest_path) ; 



/* 



GOAL REACHED UPDATE 

oOo 

This function appends the goal point to the shortest path if the last node 
of the shortest path list and the goal point are visible to each other, 
it then returns the entire list of possible paths back to the calling 
function . */ 



paths 

point 

paths 

a_path 

{ 



*goal_reached_update (goal, shortest, partial_path) 
goal; 

* shortest ; 

*partial_path; 
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a_path 

double 



new_record, *temp; 
cost = 0; 



#if def DOS 

if ( !plot2screen ) 

printf ( "\nYou have reached the goal\n") ; 

tel if 

printf ( "\nYou have reached the goal\n"); 



tendif 

for (temp = partial _jpath; t emp->next_node != NULL; 
temp = temp->next_node) 

cost += distance (temp->pt , temp->next_node->pt) ; 

temp->next_node = shortest->last_node; 
shortest->last_node = partial_path; 
cost += distance (temp->pt , t emp->next_node->pt ) ; 
shortest->cost += cost; 

NEW_NODES (new_record) ; 

new_record->pt = assign_point_values (goal) ; 

shortest->cost += distance (shortest->last_node->pt , new_record->pt ) ; 

new_record->next_node = shortest->last_node; 

shortest->last_node = new_record; 

shortest->total_cost = shortest->cost ; 

shortest->estimated_cost = 0; 

shortest->type_tangent = NONE; 

return (shortest); 

} 



/* 



FIND_TANGENTS_FROM_INNER_NODES 

oOo 

This function finds all valid tangents from an inner node to other poly- 
gons. It is very similiar to the start_f inding_paths function; however 
it checks the direction of the possible new node of the path with the 
current direction of the path to determine if the node is valid. It also 
calls a variation of cross_polygon (cross_polygon_node_on) that takes 
into consideration that both the node that form a line lie on a polygon.*/ 



paths 

paths 

polygon_list 

boundries 

active_nodes 

int 



*find_tangents_from_inner_node (shortest, start_2d, boundry, 
head_node_list , recursion_number ) 

*shortest ; 

*start_2d; 

boundry; 

*head_node_list ; 
recursion number; 



{ 



polygon_list *polygon, *previous, *polygonl; 

paths *expanded, *same_polygon = NULL; 

a_path *new_record; 



#if def DOS 

a_path *print path; 

tendif 



point ptl, pt2; 

a_path *current = shortest->last_node, 

*minus_tangent_list , 

*plus_tangent_list ; 

for (polygonl = start_2d;/* finds the polygon the last node of the 
* shortest path lies on */ 

polygonl->polygon_number != shortest->last_node->polygon_number I I 
polygonl->plane_number != shortest->plane_number ; 
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polygonl = polygonl->next_polygon) ; 



for (polygon = start_2d; 
polygon != NULL; 

polygon = polygon->next__polygon ) { 

on_same_polygon = FALSE; 

if ( shortest->plane_number < polygon->plane_number ) 
break; /^planes found after the plane which shortest path last node 
is are not checked*/ 

if (shortest->plane_number != polygon->plane_number I I 
shortest->last_node->polygon_number == polygon->polygon_number ) 
continue; 



minus_tangent_list = tangent ( shortest , polygonl, polygon->polygon_start , 
MINUS, start_2d, AROUND_PATH) ; 

plus_tangent_list = tangent (shortest, polygonl, polygon->polygon_start, 
PLUS, start_2d, AROUND_PATH) ; 



if (direction_heuristic4tangent (shortest, plus_tangent_list ) && 
boundry_check (boundry, plus_tangent_list->pt ) && 

boundry_check (boundry, plus_tangent_list->next_node->pt ) ) { 

if (NULL ! = plus_tangent_list && 

check_active_node_list (head_active_nodes , shortest, 
plus_tangent_list->pt ) ) { 



# if def DOS 

if (plot2screen) { 

for (print_path = plus_tangent_list ; print_path->next_node != 
print_path = print_path->next_node ) 
draw_line_segment (print_path->pt , print_path->next_node->pt 



NULL; 

YELLOW, 



#endif 



REGULAR) ; 

draw_line_segment (print_path->pt , shortest->last_node->pt , YELLOW, 
REGULAR) ; 



paths_f ound++; 

expanded = duplicate_paths ( shortest ) ; 

shortest = expand_paths_with_values (shortest , expanded, 

plus_tangent_list , head_node_list , 
polygon->polygon_number, PLUS) ; 

if (on_same_polygon) 
same_polygon = expanded; 

) 

) 



#if def 



if (direction_heuristic4tangent (shortest, minus_tangent_list ) && 
boundry_check (boundry, minus_tangent_list->pt ) && 
boundry_check (boundry, minus_tangent_list->next_node->pt ) ) { 

if (NULL ! = minus_tangent_list && 

check_act ive_node_list (head_act ive_nodes , shortest, 
minus_tangent_list->pt ) ) { 

DOS 

if (plot2screen) { 

f or (print_path = minus_tangent_list ; pr int_path->next_node != 
print_path = print_path->next_node) 
draw_line_segment (print_path->pt , print_path->next_node->pt , 
REGULAR); 



NULL; 

YELLOW, 



draw_line_segment (print_path->pt , shortest->last_node->pt , YELLOW, 

REGULAR) ; 



) 



#endif 



paths_f ound++; 

expanded = duplicate_paths ( shortest ) ; 
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shortest - expand_paths_with_values (shortest, expanded, 
minus_tangent_list , head_node_list , 
polygon->polygon_number , MINUS) ; 

if (on_same_polygon ) 

same_polygon = expanded; 

} 

} 

} 

return (shortest); 



/* 



CHECK FOR NODE ON POLYGON 

0O0 

This function determines if a point is on a polygon. Since all the 
polygons lie on the same plane, depth was not considered in this function 
and the z value of the point was not used. It returns a one if the node 
is on a polygon, else a zero*/ 



int check_f or_node_on_polygon (pt, current) 

point pt; 

obstacle_plane *current; 

{ 

if (pt.x == current->pt . x && 
pt.y == current->pt . y && 
pt . z == current->pt . z) 
return (1) ; 



if (pt.x == current->ccw->pt . x && 
pt.y == current->ccw->pt . y && 
pt . z == current->ccw->pt . z ) 
return ( 1 ) ; 
else 

return (0) ; 



/* 



REMOVE SHORTEST PATH 



oOo 

This function removes, clears from memory the path marked as the shortest. 
In doing so, it reestablishes the link list and returns to the calling 
function the start of the path list.*/ 



paths 

paths 

{ 

paths 

a_path 



*remove_shortest_path (shortest, start_path) 
*shortest, *start_path; 

*current, *previous; 

*current_node, *previous_node; 



for (current = start_path; 
current 1= shortest; 

previous = current, current = current->next_path) ; 

if (current == start_path) 

start__path = current->next_path; 
else { 

previous->next_path = current->next path; 

} 

current->next_path = NULL; 
previous_node = current->last_node; 
current_node = previous_node; 
free (current) ; 
do { 

current_node = current_node->next_node; 
f ree (previous_node) ; 
previous_node = current_node ; 

} while (previous_node != NULL); 



no 



return ( start_path) ; 

} 



/* 



CROSS POLYGON NODE ON 

0O0 

This function determines if a line crosses the polygon. It checks each 
line segment of the polygon and returns 1 if there is an intersection. 
The first for structure is used to check if p2 is on a polygon. If so, 
boolean is asserted to true (int value = 1) . This tells the rest of the 
for loop to go on to the next polygon*/ 



int 

polygon_list 

point 

int 



cross_polygon_node_on (start, 
*start; 
pi, p2 ; 
plane_number ; 



int 

polygon_list 

obstacle_plane 

point 

double 



count, boolean = FALSE; 
*polygon = NULL; 

* current = NULL; 
p3 , p4; 
w, x, y, z; 



pi, p2, plane_number ) 



for (polygon = start; polygon != NULL; 
polygon = polygon->next_polygon) { 



for (count = 1, current = polygon->polygon_start , boolean = FALSE; 
count <= polygon->polygon_start->number__nodes; 
count++, current = current->ccw) { 
if (polygon->plane_number != plane_number ) { 

boolean - TRUE; 
break ; 



if (p2.x == current->pt . x && p2 . y — current->pt . y ) { 

boolean = TRUE; 
break; 

} 

} 

if (’boolean) { 



for (count = 1, current = polygon->polygon_start ; 

count <= polygon->polygon_start->number_nodes ; 
count++, current = current->ccw) { 
p3.x = current->pt . x, p3.y = current->pt . y ; 
p4.x = current->ccw->pt . x, p4.y = current->ccw->pt . y ; 

if ((pl.x == current->pt . x && pl.y == current->pt . y ) || (pl.x 

== current->ccw->pt . x && pl.y -- current->ccw->pt . y ) ) 
continue ; 



if (point_on_line (current->pt , current->ccw->pt , pi)) { 

continue; /* skips over pi which is on the polygon */ 

} 



w = sign ( function_one (p3 . x, 
x = sign ( function_one (p4 . x, 
y = sign ( function_two (pi . x, 
z = sign ( function_two (p2 . x, 



p3.y, pi, 
p4.y, pi, 
pl.y, p3, 
p2.y, p3, 



p2> ) ; 
p2 ) ) ; 

p4) ) ; 
p4) ) ; 



if (w != x && y != z) 

return (1) ; /* line intersects polygon */ 



} 

current = current->ccw; 

} 



m 



return (0) ; 

} 



/* 



REVERSE LIST 

O 0o 

This function identifies the shortest path and reverses it so it is from 
start to goal vice goal to start order. The function builds a new path 
in doing so returns the path with a pointer labeled reverse path*/ 



paths 

paths 

{ 

paths 

a_path 



*reversed_list (path_list) 

*path_list ; 

*reversed_path; 

*current_path_list, *current_reversed_path, *new_record; 



reversed_path = duplicate_entries (path_list ) ; 



NEW_NODES (reversed_path->last_node ) ; 
current_reversed_path = reversed_path->last_node ; 
current_reversed_path->pt = assign_point_values (start_point ) ; 
current_reversed_path->next_node = NULL; 

for (current_path_list = path_list->last_node; 
current_path_list i= NULL; 

current_path_list = current_path_list->next_node) { 



NEW_N0DES (new_record) ; 

new_record->pt = assign_point_values (current_path_list->pt ) ; 
new_record->next_node = current_reversed_path->next_node; 
current_reversed_path->next_node = new_record; 

} 

return { reversed_path) ; 



/* 



DIRECTION HEURISTIC FROM START 

oOo 

This function determines if the new node to be added to the start paths 
list is valid. A node is valid if the direction from the start to the 
node is within 90 degrees of the start to goal direction. The initial pt 
can either be the start point or the goal point. If it is the goal pt 
then the goal direction is normalized to goal direction minus 180 degrees 
*/ 



int direction_heuristic_from_start {expand, pt) 

point expand, pt; 

{ 

double ray_direction, a, goal_dir = goal_direct ion; 

if (goal. x == expand. x && goal.y == expand. y && goal.z == expand. z) 
goal_dir = norm (goal_dir + PI); 

ray_direction = atan2((pt.y - expand. y), (pt.x - expand. x)); 
a = ABS (norm (ray_direction - goal_dir) ) ; 
if (a <= HPI ) 
return (1 ) ; 
else 

return (0) ; 



/IN- 



DIRECTION HEURISTIC 

— oOo 

This function determines if the new node to be added to a valid path is 
in the correct direction. If the type of tangent to the last node is a 
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plus tangent, the the new direction minus the old direction must be less 
than or equal to zero. Vice-a-versa for a minus tangens*/ 



int 

paths 

point 

{ 

double 

a_path 



direction_heuristic (current , pt) 

^current ; 
pt; 

ray_direction, a, goal_dir = goal_direction; 
*node = current->last_node; 



if ( ! current->f orward) 

goal_dir = norm ( goal_dir + PI) ; 



ray_direction = atan2((pt.y - node->pt.y), 
(pt.x - node->pt . x) ) ; 



a = ray_direction - goal_dir; 
a = norm (a) ; 
if (ABS(a) > HP I ) 
return (0) ; 



a = norm (ray_direction - current->ray_direction) ; 
if (ABS(a) > PI / 2) 
return (0 ) ; 

/* else 

return ( 1) ; */ 



if (current->type_tangent == PLUS) { 
if (norm (ray_direction - current->ray_direction ) <= 0) 
return (1) ; 
else 

return (0) ; 



else { 

if (norm ( ray_direction - current->ray_direction ) >= 0) 

return ( 1 ) ; 

else 

return (0) ; 

) 

) 



/* 

NORM 

ooo 

This function normalizes an angle between -PI and PI.*/ 

double norm(a) 

double a; 

{ 

while ((a > PI) || (a <= -PI)) { 

if (a > PI) 
a = a - DPI; 
else 

a = a + DP I ; 

) 

return (a) ; 

} 



/* 



UPDATE CURRENT PATH 
ooo 



This function updates the path information associated with the path 
pointed at by the current_path pointer. After updating the info, the 
function returns the start_path pointer back to the calling function.*/ 



paths 

paths 



*update_current_path (current_path, / *start_path, */ tangent, 
start_pt, pt, goal, polygon_number ) 
*current_path/* , *start_path*/; 
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point 

int 



start_pt, pt , goal; 
tangent, polygon_number ; 



{ 

current_path->last_node->pt = assign_point_values (pt ) ; 
current_path->last_node->polygon_number = polygon_number ; 
current_path->last_node->next_af ter_node = NULL; 
current_path->cost = distance { start_pt , pt); 
current_path->estimated_cost = distance (goal, pt) ; 
current_path->total_cost = TOTAL_COST; 
current_path->type_tangent = tangent; 

current_path->ray_direction = atan2((pt.y - start_pt.y), 

(pt.x - start_pt . x) ) ; 

current_path->plane_number = plane_number; 
current_path->able2expand = TRUE; 
current_path->f orward = YES; 

head_active_nodes = insert_nodes_into_active_node_list (head_active_nodes , pt 

current_path->cost ) ; 

current_path->when_path_f ound = paths_f ound++; 
return (current_path) ; 



/* 

EXPAND PATHS WITH VALUES 

oOo 

This function recalculates the header data of the duplicate record of the 
shortest path to include the new point to be added (hence expanded as the 
variable name) . It also inserts this expanded path into the link list of 
valid paths after the shortest path and returns the shortest path to the 
calling function.*/ 

paths *expand_paths_with_values (shortest , expanded, tangent_list , 

head_active, polygon_number , mode) 
paths *shortest, *expanded; 

a_path * tangent_list ; 

active_nodes *head_active; 
int polygon_number , mode; 

{ 

a_path *current = shortest->last_node , *to_polygon 

tangent_list->next_node ; 

int last_tangent_type = expanded->type_tangent ; 

if ( shortest ->last_node->pt . x == to_polygon->pt . x && 
shortest->last_node->pt . y == to_polygon->pt . y && 
shortest->last_node->pt . z == to_polygon->pt . z ) { 

expanded->cost += distance ( shortest->last_node->pt , tangent_list->pt) ; 
free (to_polygon) ; 

tangent_list->next_node = expanded->last_node ; 
expanded->last_node->next_after_node = tangent_list ; 
expanded->last node = tangent list; 

} 

else { 

expanded = connect_links_of_path_on_polygon (expanded, tangent_list 
last_tangent type); 

} 



/* 

expanded->cost += distance (shortest->last_node->pt 
shortest->last_node->next node->pt) ; 

V 

if (expanded->forward) 

expanded->estimated_cost = distance (goal, expanded->last_node->pt ) ; 
else 

expanded->estimated_cost = distance ( start_point , expanded->last_node->pt ) ; 
expanded->total_cost = expanded->cost + expanded->estimated_cost ; 
expanded->ray_direct ion = atan2 ( (expanded->last_node->pt . y - 

expanded->last_node->next_node->pt . y ) , 
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(expanded->last_node->pt . x - 
expanded->last_node->next_node->pt . x) ) ; 
expanded->able2expand = TRUE; 
expanded->type_tangent = mode; 

head_active = insert_nodes_into_active_node_list (head_acti ve , 

expanded->last_node->pt , expanded->cost ) ; 



/* 

expanded = update (expanded, mode) ; 

*/ 

expanded->when_path_found = paths_f ound++; 

expanded->next_path = shortest->next_path; 
expanded->last_node->polygon_number = polygon_number ; 
shortest->next_path = expanded; 

return (shortest); 

} 



/* 



ADD ON NEW PLANE INTERSECTION 
000 



This function finds the vertical and perpendicular planes at the point 
passed in. The point is the vertical point to be expanded from. Once 
it finds these planes, the function calls the build_two_d_polygon_list , 
which determines the polygons formed by the intersection of the new 
perpendicular plane with each polyhedron in the search space.*/ 



polygon_list 

polygon_list 

a_path 

point 



*add_on_new_plane_intersection (start_2d, 

vert ical_pt_list , 

*start_2d; 

* vert ical_pt_list ; 
goal; 



polygon_list * last_polygon ; 

a_path *current = vertical_pt_list ; 



goal) 



vertical_n_perpendicular_plane ( vertical_pt_list->pt , goal) ; 
start_2d = build_two_d_polygon_list (start_2d, perpendicular, TRUE); 



return (start_2d); 

} 



/* 

POINT ON POLYGON 

ooo 

This function determines if a point lies on a line. The function is used 
to skip the polyhedron edge which the vertical point expanded from lies 
on. It is also is used as a heuristic to determine if a new vertical 
point lies between the goal point and the current point being expanded 
on . */ 

int point_on_line (ptl , pt2, pt3) 

point ptl, pt2, pt3;/* ptl and pt2 are end points of 

* line, pt3 is point being checked */ 

{ 

double tl, t2 , t3; 

if (pt2.x == ptl.x) { 
if (pt3 .x != ptl.x) 
return (0) ; 
else { 

if ( (ptl . y ! = pt2.y) && (ptl.z != pt2.z)) { 
t2 = (pt3.y - ptl.y) / (pt2.y - ptl.y); 
t3 = (pt3.z - ptl.z) / (pt2.z - ptl.z); 
if (t2 == t3) 
return ( 1) ; 
else 
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return (0) ; 



} 

else { 

if (ptl .y == pt2 . y ) { 

if (pt3 . y == ptl .y) 
return ( 1 ) ; 
else { 

t3 = (pt3.z - ptl.z) / (pt2.z - ptl.z); 
if (0 <= t3 && t3 <= 1) 
return ( 1 ) ; 
else 

return (0) ; 

} 

} 

else { 

if (ptl.z == pt2 . z ) { 

if (ptl . z != pt3 . z) 
return (0) / 
else { 

t2 = (pt3 . y - ptl.y) / (pt2.y - ptl.y) 
if (0 <= t2 && t2 <= 1) 
return (1) ; 
else 

return (0 ) ; 

} 

} 

} 

} 

} 

} 

else { 

if (pt2.y == ptl.y) { 
if (pt 3 . y != ptl .y) 
return (0 ) ; 
else { 

if (ptl.z != pt2.z) { 

tl = (pt3.x - ptl.x) / (pt2.x - ptl.x}; 
t3 = (pt3.z - ptl.z) / (pt2.z - ptl.z); 
if (tl == t3) 
return ( 1 ) ; 
else 

return (0 ) ; 

} 

else { 

if (ptl . z == pt2 . z) { 
if (pt3 . z == ptl.z) 

tl = (pt3.x - ptl.x} / (pt2.x - ptl.x) 
if (0<=tl&&tl<=l) 
return (1}; 
else 

return (0} ; 

} 

} 

} 

} 

else { 

if (ptl.z == pt2 . z ) { 

if (pt3 . z ! = pt2 . z ) 
return (0) ; 

} 

else { 

tl = (pt3 . x - ptl.x) / (pt2 . x - ptl.x); 
t 2 = (pt3.y - ptl.y) / (pt2.y - ptl.y); 
if (tl == t2) 
return ( 1) ; 
else { 

tl = (pt3.x - ptl.x) / (pt2.x - ptl.x); 
t2 = (pt3 . y - ptl.y) / (pt2.y - ptl.y); 
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t3 = (pt3.z - ptl.z) / (pt2.z - ptl.z); 
if (tl == t2 == t3 ) 
return ( 1 ) ; 
else 

return (0 ) ; 

} 

} 

} 

} 

return (0 ) ; 



/* 



CONNECT LINKS OF PATH ON POLYGON 



oOo 

This function connects the links of a path on the polygon. The tangent 
function finds the either a plus or minus tangent between polygons. This 
tangent connects the path between the last node of the expanded path and 
the tangent node which lies on the same polygon as that last node.*/ 



paths 

paths 

a_path 

int 



*connect_links_of_path_on_polygon (expanded, tangent_list , 
*expanded; 

*tangent_list ; 
mode ; 



a_path * last_node_in_tangent = tangent_list->next_node ; 

polygon_list *polygon; 
obstacle_plane *node; 
point extend; 



mode ) 



extend = expanded->last_node->pt ; 

last_node_in_tangent->polygon_number m expanded->last_node->polygon_number ; 

/* finds correct polygon points lie on */ 
for (polygon = start_2d; 

polygon->polygon_number != expanded->last_node->polygon_number ; 
polygon = polygon->next_polygon) ; 



/* 

* finds the correct node in polygon which is equal to last node in 

* tangent list 
*/ 

for (node = polygon->polygon_start ; 

node->pt.x i= last_node_in_tangent->pt . x || 
node->pt.y != last_node_in_tangent->pt . y || 
node->pt . z != last_node_in_tangent->pt . z; 
node = node->ccw) ; 

while (LOOPFOREVER) { 
if (mode == PLUS) { 

if (extend. x == node->cw->pt . x && extend. y == node->cw->pt . y && extend. z == 
node->cw->pt . z) 
break; 

else { 

NEW_NODES (last_node_in_tangent~>next_node ) ; 

last_node_in_tangent->next_node->next_after_node = last_node_in_tangent ; 
last_node_in_tangent = last_node_in_tangent->next_node; 
last_node_in_tangent->pt = assign_point_values (node->ccw->pt) ; 

node = node->ccw; 

} 

else { 

if (extend. x == node->ccw->pt . x && extend. y == node->ccw->pt . y && extend. z 
== node->ccw->pt . z) 
break; 
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else { 

NEW_NODES ( last_node_in_tangent->next_node ) ; 

last_node_in_tangent->next_node->next_after_node = last_node_in_tangent 
last~node_in__tangent = last_node_in_tangent->next_node; 
last_node_in_tangent->pt = assign_point_values (node->cw->pt ) ; 

} 

node = node->cw; 

} 

} 

expanded->cost = calculate_cost ( tangent_list , expanded->last_node, 

expanded->cost , last_node_in_tangent ) ; 
last_node_in_tangent->next_node = expanded->last_node; 
expanded->last_node->next_af ter_node = last_node_in_tangent ; 
expanded->last_node = tangent_list ; 
return (expanded) ; 

} 



double 

a_path 

double 

{ 

a_path 



calculate_cost ( list , last_node_in_path, cost, last_tangent ) 
*list, *last_node_in_path, *last_tangent ; 
cost ; 

* tangent = last_tangent ; 



cost += distance (last_node_in_path->pt , last_tangent->pt ) ; 

head_active_nodes = insert_nodes_into_active_node_list (head_act ive_nodes , 

last_tangent->pt , cost) / 



for (tangent = last_tangent->next_after_node; 
tangent ! = NULL; 

tangent = tangent ->next_after_node) { 

cost += distance (tangent->pt, tangent->next_node->pt) ; 

head_active_nodes = insert_nodes_into_act ive_node_list (head_act ive_nodes, 

tangent->pt, cost) ; 



} 

return (cost); 



/ * oOo* 

MARK SHORTEST LAST NODE 



This function marks the last node's polygon number with the polygon's 
number in which the node was found. 

*/ 

paths *mark_shortest_path_last_node (shortest , start2) 
paths *shortest; 
polygon_list *start2; 

{ 

polygon_list ^polygon; 
obstacle_plane *node; 
int count; 

for (polygon = start2; 
polygon != NULL; 

polygon = polygon->next_polygon) { 

if (shortest->plane_number != polygon->plane_number ) 
continue; 

for (count = 1, node = polygon->polygon_start; 
count <= polygon->polygon_start->number_nodes ; 
node = node->ccw, count++) { 

if (node->pt.x != shortest->last_node->pt . x || 
node->pt.y != shortest->last_node->pt . y I I 
node->pt.z != shortest->last_node->pt . z ) 
continue; 
else{ 
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shortest->last_node->polygon_number = polygon->polygon_number ; 
break ; 



} 

} 

return (shortest) ; 



/* 

UPDATE 

000 

This function accepts a path which has been expanded on and updates 
the header information accordingly, passing back the path and updated 
header info 
*/ 

paths *update (the_path, tan) 
paths *the_path; 
int tan; 

{ 

the_path->cost += distance (the_path->last_node->pt , 
the_path->last_node->next_node->pt ) ; 
the_j?ath->estimated_cost = distance (the_path->last_node->pt , goal); 
the_path->total_cost = the_path->cost + the_path->est imated_cost ; 

the_path->ray_direction = atan2 ( (the_path->last_node->pt . y - 

the_path->last_node->next_node->pt . y) , 
(the_path->last_node->pt . x - 
the_path->last_node->next_node->pt . x) ) ; 
the_path->able2expand = TRUE; 
the_path->type_tangent = tan; 

head_active_nodes = insert_nodes_into_active_node_list (head_act ive_nodes, 

the_path->last_node->pt , the_path->cost ) ; 

the_path->when_path_f ound = paths_f ound++; 

the_path->next_path = the_path->next_path ; 

return (the_path) ; 



int direction_heuristic4tangent (path, list) 
paths *path; 
a_path *list; 

{ 

double direction, goal_dir = goal_direction , a; 
a_path *node = path->last_node; 

if ( !path->f orward) 

goal_dir = norm (goal_dir + PI); 

direction = atan2 ( ( list->pt . y - list->next_node->pt . y ) , 
(list->pt.x - list->next_node->pt . x) ) ; 

a = direction - goal_dir; 
a = norm (a) ; 
if (ABS(a) > HP I ) 
return (0 ) ; 

if (node->pt . x == list->pt.x && 
node->pt.y == list->pt.y && 
node~>pt.z == list->pt.z){ 

a = norm(path->ray_direction - direction) ; 
if (ABS (a) > HP I ) 
return (0) ; 
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} 

return ( 1 ) ; 
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APPENDIX G 



This appendix contains the source code to make the simple plots of the world. This 
source code is DOS dependent and is not included if the defined variable DOS is 
equal to 1 during compilation. 



^include "3d_tan.h" 
#include "plot.h" 

# ifdef DOS 



#define ESC 
#def ine SCALEDX 
#def ine SCALEDY 
#def ine DOT 



Oxlb/* Define the escape key*/ 
x / 48 
y / 34 

1 /*used to make a dot with radius 1*/ 



void data_plot ( start_2d ) 
polygon_list *start_2d; 

{ 

int x, y, z, w; 

Initialize( ) ; 

MainWindow( " PLOT OF THE TWO D ENVIRONMENT" ); 



StatusLine( "PRESS ANY KEY TO END" ); 
x = getmaxx ( ) ; 
y = getmaxy ( ) ; 

z = start_point .x * SCALEDX; w = start_point . y * SCALEDY; 

outtextxy( z, w, "X" ); 

outtextxy ( z+20 , w+15, "START"); 

z = goal . x * SCALEDX; w = goal.y * SCALEDY; 

outtextxy( z, w, "X" ); 

outtextxy(z + 20, w+15, "GOAL"); 

draw_boundry ( ) ; 

draw_poly gons ( star t_2d ) ; 



] 



void Initialize (void) 

{ 

int xasp, yasp; /* Used to read the aspect ratio*/ 

if ( registerbgidr iver ( EGAVGA_dr iver ) < 0) /*checks for correct driver*/ 
exit ( 1 ) ; 

if ( registerbgifont ( triplex_font ) < 0) /*checks for correct font*/ 
exit ( 1 ) ; 

GraphDriver = DETECT; /* Request auto-detection*/ 

initgraph( &GraphDriver , &GraphMode, ""); 

ErrorCode = graphresult ( ) ; /* Read result of initialization*/ 

if( ErrorCode != grOk ) {/* Error occured during init*/ 

printf(" Graphics System Error: %s\n", grapherrormsg ( ErrorCode ) ); 
exit ( 1 ) ; 

} 

getpalette( &palette ); /* Read the palette from board*/ 

MaxColors = getmaxcolor ( ) + 1;/* Read maximum number of colors*/ 

MaxX = getmaxx(); 

MaxY = getmaxy(); /* Read size of screen*/ 
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} 



getaspectratio ( &xasp, &yasp );/* read the hardware aspect*/ 
AspectRatio = (double)xasp / (double)yasp; /* Get correction factor*/ 



/* V 

/* PAUSE: Pause until the user enters a keystroke. If the*/ 
/* key is an ESC, then exit program, else simply return.*/ 

/* V 



void Pause(void) 

( 

static char msg[] = M Pausing. Esc aborts or press a key..."; 
int c; 



StatusLine( msg ) ; 



/* Put msg at bottom of screen*/ 



c = getch( ) ; 



/* Read a character from kbd*/ 



if ( ESC == c ) [ 
closegraph( ) ; 
exit ( 1 ) ; 

] 



/* Does user wish to leave?*/ 
/* Change to text mode*/ 

/* Return to OS */ 



if ( 0 == c ) { 
c = getch( ) ; 

} 

StatusLine( "Working on 



/* Did use hit a non-ASCII key? */ 
/* Read scan code for keyboard*/ 

next expans ion " ) ; 



/*cleardevice( ) ; */ 



/* Clear the screen*/ 



} 



void MainWindow( char *header ) 

{ 

int height; 

cleardevice( ) ; /* Clear graphics screen*/ 

setcolor( MaxColors - 1 );/* Set current color to white*/ 
setviewport( 0, 0, MaxX, MaxY, 1 );/* Open port to full screen*/ 

height = textheight( "H" ); /* Get basic text height */ 

changetextstyle( DEFAULT_FONT , H0RIZ_DIR, 1 ); 
settext justify ( CENTER_TEXT, T0P_TEXT ); 
outtextxy( MaxX/2, 2, header ); 

setviewport( 0, height+4, MaxX, MaxY- ( height+4 ) , 1 ); 

DrawBorder ( ) ; 

setviewport ( 1, height+5, MaxX-1, MaxY- ( height+5 ) , 1 ); 



} 

/* V 

/* DRAWBORDER: Draw a solid single line around the current */ 
/* viewport. */ 

/* V 

void DrawBorder (void) 

[ 

struct viewporttype vp; 

setcolor( MaxColors - 1 );/* Set current color to white*/ 
setlinestyle( SOLID_LINE, 0, NORM_WIDTH ); 
getviewsettings ( &vp ); 

rectangle( 0, 0, vp . right- vp . left , vp . bottom- vp . top ); 
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/* 

/* 

/* 



V 

STATUSLINE: Display a status line at the bottom of the screen.*/ 

V 

void StatusLine( char *msg ) 

{ 

int height; 

setviewport( 0, 0, MaxX, MaxY, 1 );/* Open port to full screen*/ 
setcolor( MaxColors - 1 );/* Set current color to white*/ 

changetextstyle( DEFAULT_FONT , HORIZ_DIR / 1 ); 
set text justify ( CENTER_TEXT , TOP_TEXT ); 
setlinestyle( SOLID_LINE, 0, NORM_WIDTH ); 
setf ills tyle( EMPTY_FILL, 0 ); 

height = textheight( "H" ); /* Detemine current height 

bar( 0, MaxY- (height+4 ) , MaxX, MaxY ); 

rectangle( 0, MaxY -( height+4 ) , MaxX, MaxY ); 

outtextxy( MaxX/2, MaxY- ( height+2 ) , msg ); 

setviewport( 1, height+5, MaxX-1, MaxY- (height+5 ) , 1 ); 



} 

void changetextstyle ( int font, int direction, int charsize) 

{ 

int ErrorCode; 



graphresult ( ) ; /* clear error code*/ 

settextstyle ( font , direction, charsize); 

ErrorCode = graphresult (); /* check result */ 
if( ErrorCode 1 = grOk ) [/* if error occured*/ 
closegraph ( ) ; 

printf(" Graphics System Error: %s\n", grapherrormsg ( ErrorCode ) ); 
exit ( 1 ) ; 

) 

} 



void draw_boundry ( ) 
{ int x, y; 



x “ getmaxx(); 
y = getmaxy ( ) ; 



setlines tyle(SOLID_LINE, 0, 



setcolor ( 4 ) ; 
line(boundry . xl 
boundry . y2 
line(boundry . xl 
boundry . yl 
line (boundry . x2 
boundry . y2 
line (boundry . xl 
boundry . y2 

} 



* SCALEDX , 

* SCALEDY) 

* SCALEDX, 

* SCALEDY) 

* SCALEDX, 

* SCALEDY) 

* SCALEDX, 

* SCALEDY) 



THICK WIDTH) ; 



boundry . yl 
boundry . yl 
boundry . yl 
boundry . y2 



* SCALEDY, 

* SCALEDY, 

* SCALEDY, 

* SCALEDY, 



boundry. xl * SCALEDX, 
boundry. x2 * SCALEDX, 
boundry. x2 * SCALEDX, 
boundry. x2 * SCALEDX, 



void draw^polygons (start_2d) 
polygon_list *start_2d; 

t 

polygon_list *poly; 
obstacle_plane *node; 
int count = 1, x, y; 



x = getmaxx ( ) ; 
y = getmaxy ( ) ; 



setlines tyle ( SOLID_LINE, 0, NORM_WIDTH) ; 

for (poly = start_2d; poly != NULL; poly = poly ->next_polygon ) { 
for (node = poly->polygon_start , count = 1; 

count <= poly- >polygon_start->number_nodes ; 
node = node->ccw, count++) [ 



} 



line( node->pt . x * SCALEDX , node->pt.y * SCALEDY , 

node->ccw->pt . x * SCALEDX, node- >ccw- >pt . y * SCALEDY); 



void draw_shortest_path( shortest ) 
paths ^shortest; 

( 

paths *path; 

a_path *point; 

int x, y, w, z, style; 

x = getmaxx ( ) ; 
y = getmaxy ( ) ; 
style = THICK_WIDTH; 
setcolor ( 14 ) ; 

setlinestyle(SOLID_LINE, 0, THICK_WIDTH) ; 
for(point = shortest-> last_node; point != NULL; 
point = point->next_node) ( 
if ( point->next_node != NULL) 

draw_line_segment (point->pt, point-> next_node->pt , YELLOW, style); 

/* (point->pt . x * SCALEDX, point->pt.y * SCALEDY, 

point->next_node- >pt . x * SCALEDX, point->next_node->pt . y * SCALEDY);*/ 
circle( point->pt . x * SCALEDX, point->pt.y * SCALEDY, DOT); 

) 

Pause( ) ; 
cleardevice ( ) ; 



void draw_line_segment ( ptl , pt2, color, style) 
point ptl, pt2; 
int color, style; 

( 

int x, y, w, z; 

x = getmaxx ( ) ; 
y = getmaxy ( ) ; 
setcolor ( color ) ; 
if(color == RED || style) 

setl ines ty le ( SOLID_LINE, 0, THICK_WIDTH) ; 
else 

setl ines tyle ( DOTTED_LINE, 0, NORM_WIDTH); 
line(pt2.x * SCALEDX, pt2.y * SCALEDY, ptl.x * SCALEDX, ptl . y * SCALEDY); 
z = ptl.x * SCALEDX; w = ptl . y * SCALEDY; 
outtextxy( z, w, "X" ); 

z = pt2 . x * SCALEDX; w = pt2.y * SCALEDY; 
outtextxy( z, w, "X" ); 



void id_shortest_path_for_expans ion ( shortest) 
paths ^shortest; 

( 



int x, y, color, i; 
point pi, p2; 
a_path *node; 

x = getmaxx(); 
y = getmaxy ( ) ; 
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setcolor ( BLACK ) ; 

outtextxy ( Expansion_lable_x * SCALEDX, 

Expans ion_lable_y * SCALEDY, "X" ); 
outtextxy (Expans ion_lable_x * SCALEDX + 20, 

Expans ion_lable_y * SCALEDY + 15, ’’EXPAND" ); 

for(i = 1, node = shortest->last_node; 
node->next_node ! = NULL; 
node = node->next_node, i++){ 
pi = node->pt; 
p2 = node->next_node->pt; 

draw_line_segment (pi, p2, RED, REGULAR); 

if(i “ 1){ 

Expansion_lable_x = pl.x; 

Expansion_lable_y = pl.y; 

outtextxy ( pl.x * SCALEDX, pl.y * SCALEDY, "X" ); 
outtextxy (pi .x * SCALEDX + 20, pl.y * SCALEDY + 15, 

3 

3 

Pause () ; 



3 

#endi f 



"EXPAND" ) 
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APPENDIX H 



This appendix contains the source code in print. c and has only those functions 
associated with printing to either a file or the screen. It also contains some 
print utilities I used in debugging other portions of my code 

#include n 3d_tan.h" 

void print_polyhedron ( list ) 

obstacle_list *list; 

{ 

obstacle_list *current_obstacle = polyhedron_list ; 
polyhedron_obstacle *current_f ace; 
polyhedron_obstacle_plane * cur rent ; 
int count l f count2, count3; 

for (countl = 1, current_obstacle = list; 
count 1 <= number_polygons; 
count 1 + + , 

current_obstacle = current_obstacle->next_joolygon ) { 

print f ( "XnObstacle %d\n", countl); 

for (current_face = current_obstacle->obstacle, count2 = 1; 
current_face != NULL; 

current_face = current_f ace->next_f ace , count2++) { 
printf ( M \nplane face %d\n", count2); 

for (current = current_f ace->f ace_nodes, count3 = 1; 

count3 <= current_f ace->f ace_nodes->number_nodes; 
count3++, current = current->ccw) { 

printf("\nx = %.31f, y = %.31f, z = %.31f", 

current->pt.x, current->pt . y , current->pt . z) ; 

} 

} 

} 

} 



/* 



PRINT RESULTS 



oOo 

void print_results (polyhedron_list , number_polygons, 

start_point, goal, plane_equation) 
obstacle_l ist *polyhedron_list ; 
equation plane_equation; 

point start_point, goal; 



int countl, count2, count3; 

obstacle_list *current_obstacle = polyhedron_list ; 
polyhedron_obstacle *current_f ace ; 
polyhedron_obstacle_plane ‘current; 

polyhedron_obstacle_plane *ptl = NULL, *pt2 = NULL, *pt3 = NULL; 



for (countl = 1, current_obstacle = polyhedron_list; 
countl <= number_polygons ; 
count 1++, 

current_obstacle = current_obstacle->next_polygon) { 
printf ( "XnObstacle %d\n", countl); 



for (current_f ace = current_obstacle->obstacle, count2 = 1; 
current_face != NULL; 

current_face = current_f ace->next_f ace , count2++) { 
printf ( "\nplane face %d\n", count2); 



V 
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for (current = current_face->face_nodes, count3 = 1; 

count3 <= cur rent_f ace->f ace_nodes->number_nodes ; 
count3++, current = current->ccw) { 

printf("\nx = %.31f, y = %.31f, z = %.31f", 

current->pt . x, current->pt .y, current->pt . z ) ; 

} 

} 



/ * check * / 

printf ( "Xnstart point is %.31f, %.31f, %.31f\n", 
start_point . x, start__point . y, start_point . z ) ; 
pr intf ( "\ngoal is %.31f, %.31f, %.31f\n", 
goal.x, goal.y, goal.z); 

/* check plane equation for first face */ 
current_face = polyhedron_list->obstacle ; 
for (count2 = 1, current_obstacle = polyhedron_list ; 
current_obstacle != NULL; 

current_obstacle = current_obstacle->next__polygon , 
current_face = polyhedron_list->obstacle} { 

for (countl = 1, current_face = current_obstacle->obstacle; 
current_face != NULL; 

current_face = current_face->next_face, countl++) { 

printf ( " face %d\n", countl}; 
ptl = current_f ace->face_nodes ; 
pt2 = ptl->ccw; 
pt3 = pt2->ccw; 

printf("\nx = %.31f, y = %.31f, z = %.31f", 

ptl->pt.x, ptl->pt.y, ptl->pt.z}; 
printf ( "\nx = %.31f, y = %.31f, z = %.31f ,, / 

pt2->pt.x, pt2->pt.y, pt2->pt.z}; 
printf ( "\nx = %.31f, y = %.31f, z = %.31f", 

pt3->pt.x, pt3->pt.y, pt3->pt.z}; 

plane_equation = f ind_plane_equation (ptl->pt , pt2->pt, 

pt3->pt } ; 

printf ( "plane equation = <%.3f, %.3f, %.3f, %.3f>\n", 
plane_equation . x, plane_equat ion . y, 
plane_equation . z, plane_equation . d) ; 

} 

} 

} 



/* 



PRINT 2D PATH 
oOo 



void print_2d_path (start_two_d) 

polygon_list *start_two_d; 

{ 

polygon_list *current_polygon = start_two_d; 

obstacle_plane ^current; 

int countl, count2; 

for (countl = 1; current_polygon I = NULL; 

current_polygon = current_polygon->next_polygon, countl++) { 
if (current_polygon->polygon_start == NULL) 
continue; 

print f ( "\nPOLYGON %d\nprint ccw\n", countl); 

for (count2 = 1, current = current_polygon->polygon_start ; 
count2 <= current_polygon->polygon_start->number_nodes; 
count2++, current = current->ccw) { 



*/ 
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printf ("x coord = %8.31f, y coord = %8.31f, z coord = %8.31f\n H , 
current->pt . x, current->pt . y, current->pt . z ) ; 

} 

printf ( "print cw\n" ) ; 

for <count2 » 1, current = current_polygon->polygon_start ; 
count2 <= current_polygon->polygon_start->number_nodes; 
count2++, current = current->cw) { 
printfC’x coord = %8.31f, y coord = %8.31f, z coord = %8.31f\n", 
current->pt.x, current->pt . y, current->pt . z ) ; 

} 

} 

} 

/* 

PRINT VERT PERP PATH 

oOo */ 

void print_vert_perp_plane ( ) 

{ /* all variables are globals */ 

printf ("\nThe line intersects the polygon at (%.3f, %.3f, %.3f)", 
intersection . x, intersection . y, intersection . z) ; 
printf ( "\nThe vertical plane through the start and goal is\n") ; 
printf ("%.3fl + %.3fJ + %.3fK * %.3f", 

vertical. x, vertical. y, vertical. z, vertical. d); 
printf ( H \nThe plane through the start and goal perpendicular to the"); 
printf (" vertical plane is\n %.3fl + %.3fJ + %.3fK * %.3f", 

perpendicular .x, perpendicular . y, 
perpendicular . z , perpendicular . d) ; 



/* 

PRINT EACH PATH 

oOo * / 



void 

paths 

{ 

paths 

int 



print_each_path (start_paths, iterat ion_count ) 
*start_paths; 

*current_path; 

count; 



if ( !plot2screen I I print2file) { 
if <print2file) 
if(l == iteration_count) 

fprintf (fpt2, "\n INITIAL PARTIAL PATHS\n" ) ; 
else 

fprintf (fpt2 f "\n\nPATH FINDING ITERATION COUNT = %d", iteration_count+ + ) ; 
for (current_path = start_paths, count = 1; 
current_path != NULL; 

count++, current_path = current_path->next_path) { 

if ( Iplot2screen) 

printf ( "\n\nPATH %d\n", count); 
if (print2file) 

fprintf (fpt2, "\n\nPATH %d\n", count); 

print_path (current_path) ; 

} 

} 

} 

/* 

PRINT INTERSECTION 

oOo */ 

void print_intersection (plane_equation f intersection, ptl, pt2) 

equation plane_equation; 

point ptl, pt2, intersection; 

{ 

printf ( M \nThe intersection of the line,"); 



128 



printf ("\n (% .3f, % . 3f , %.3f) to (%.3f,%.3f, %.3f)\nand the plane", 
ptl.x, ptl.y, ptl.z, pt2 . x, pt2.y, pt2.z); 
printf (" (%.3fl + %.3fJ + %.3fK + %.3f) is (%.3f, %.3f, %.3f)", 
plane_equation . x, plane_equat ion . y, 
plane_equation . z, plane_equation . d, 
intersection . x, intersection . y, intersection . z ) ; 



/* 



PRINT PATH 



0O0 



void print_path (current_path) 

paths *current_path; 

{ 

a_path *node; 

if ( !plot2screen) { 

printf { "PATH NUMBER %d ", current_path->when_path_f ound) ; 
if (current_path->able2expand) 

printf ("Path able to extend\n"); 
else 

printf ("Path cannot be extended\n") ; 

if (current_path->type_tangent == PLUS) 
printf ( "\nPlus Tangent, direction - %4.31f\n", 
current_path->ray_direction) ; 

else 

print f ( "\nMinus Tangent, direction = %4.31f\n", 
current_path->ray_direction ) / 
for (node = current_path->last_node; 
node I = NULL; 
node = node->next_node ) { 

if (node->next_node != NULL) { 

printf (" (%3.21f, %3.21f, %3.21f) to", 

node->pt.x, node->pt.y, node->pt.z); 
print f ( "\ncost = %4.31f, est.cost = %4.31f, total = %4.31f", 
current_path->cost , current_path->estimated_cost , 
current_path->total_cost) / 



*/ 



} 

else { 

printf (" (%3.21f, %3.21f, %3.21f)", 

node->pt.x, node->pt.y, node->pt.z); 
print f ( "\ncost = %4.31f, est.cost = %4.31f, total = %4,31f", 
current_path->cost , current_path->estimated_cost , 
current_path->total_cost ) ; 

} 

} 

) 

if (print2file) 

print_path_to_f ile (current_path) ; 



} 



void 

paths 



print_path_to_f ile (current_path) 
*current_path; 



{ 

a_path 



*node; 



fprintf (fpt2, "PATH NUMBER %d ", current_path->when_path_f ound) ; 
if (current_path->able2expand) 

fprintf ( fpt2, "Path able to extend\n") ; 
else 

fprintf (fpt2, "Path cannot be extended\n") / 
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fprintf ( fpt2 , "cost = %4.31f, est.cost = %4.31f, total = %4.31f\n", 
current_path->cost, current_path->estimated_cost , 
current_path->total_cost) ; 

if (current_path->type_tangent == PLUS) 

fprintf (fpt2, "\nPlus Tangent, direction = %4.31f\n", 
current_path->ray_direction) ; 

else 

fprintf (fpt2, "\nMinus Tangent, direction = %4.31f\n", 
current_path->ray_direction) ; 
for (node = current_path->last_node; 
node != NULL; 
node = node->next_node) { 
if (node->next_node != NULL) { 
fprintf (fpt2, "(%3.21f, %3.21f, %3.21f) to\n", 
node->pt.x, node->pt.y, node->pt.z); 

} 

else { 

fprintf (fpt2, "(%3.21f, %3.21f, %3.21f)\n", 
node->pt.x, node->pt.y, node->pt.z); 

} 

} 

} 



/* 

PRINT PATH SHORTEST 

0O0 

void print_path_shortest (shortest) 

paths *shortest; 

{ 

a_path ‘current; 

if ( !plot2screen) 

printf ( "\nSHORTEST PATH"); 
if (print2file) 

fprintf (fpt2, "SHORTEST PATH\n" ) ; 

for (current = shortest->last_node; 
current != NULL; 
current = current->next_node) { 
if ( !plot2screen) { 

printf (" \n (%3 . 21 f, %3.21f, %3.21f) ", 

current->pt . x, current->pt . y , current->pt . z ) ; 
if ( curr ent->next_node 1= NULL) 
printf ( " to" ) ; 

} 

if (print2file) { 

fprintf (fpt2, "\n(%3.21f, %3.21f, %3.21f)", 

current->pt . x, current->pt .y , current->pt.z) ; 
if (current->next_node != NULL) 
fprintf (fpt2, " to"); 

} 

} 

if ( !plot2screen) 

printf ( "\nThe total length of the shortest path is %4.31f\n", 
shortest->total_cost ) ; 
if (print2file) 

fprintf ( fpt2 , ”\nThe total length of the shortest path is %4.31f\n", 
shortest->total cost) ; 



*/ 
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APPENDIX I 



This function contains the source code found in tangent. c and contains only those 
functions which are associated with tangents. 



#include "3d_tan.h" 

/* oOo 

TANGENT 



oOo 

This function finds the tangent if one exist between two polygons. If any 
obstacles lie between the two polygons, then the function returns NULL. If a 
tangent line is found then the points are placed in a list in euclidean order and 
returned . 

V 

a_pa th *tangent( shortest, polygon, second_polygon_start , mode, 

start_2d, type_path, mode2) 
paths *shortest; 

polygon_list ^polygon ; 
obs tacle_plane *second_polygon_start ; 
int mode; 

polygon_list *start_2d; 
int type_path; 

int mode2; 



a_path 

point 

int 



* tangent_path_list = NULL; 
to_polygon_pt, from_polygon_pt ; 
i, points_found = 0, tan; 



if ( type_path) 

tan = shortest->type_tangent ; 
else 

tan = shortest->polygon4vert_PTR->type_tangent ; 



if ( type_path) mode2 = shortest- >type_tangent; 



if (mode2 /*shortest- >type_tangent*/ == MINUS) { 
if (mode == MINUS) [ 
for(i = 1; 

i <= second_polygon_start->number_nodes && 
i <= polygon- >polygon_start->number_nodes ; 

i ++ ) { 



to_polygon_pt = minus_tangent ( second_polygon_start , 
shortest- >last_node->pt) ; 

f rom_polygon_pt = plus_tangent (polygon->polygon_start , to_polygon_pt ) ; 
if ( ! cross_polygon_node_on ( start_2d, to_polygon_pt , f rom_polygon_pt , 
polygon- >plane_number ) ) { 
points_found = TRUE; 

break; 

} 

) 

) 

else { 

for(i = 1; 

i <= second_polygon_start->number_nodes && 
i <= polygon- >polygon_start->number_nodes ; 
i++) { 

to_polygon_pt = plus_tangent ( second_polygon_start , 
shortest->last_node->pt ) ; 

f rom_polygon_pt = plus_tangent (polygon- >polygon_start, to_polygon_pt ) ; 
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if ( ! cross_polygon_node_on ( start_2d, to_polygon_pt , f rom_polygon_pt , 
polygon- >plane_number ) ) { 
points_found = TRUE; 

break ; 

] 

) 

) 

} 

else { 

if (mode == MINUS) { 
for(i = 1; 

i <= second_polygon_start->number_nodes && 
i <= polygon- >polygon_start->number_nodes ; 
i++) [ 

to_polygon_pt = minus_tangent ( secondpolygonstart , 

shortest->last_node->pt ) ; 

from_polygon_pt = minus_tangent ( polygon- >polygon_start , to_polygon_pt ) 
if ( I cross_polygon_node_on ( start_2d, to_polygon_pt , f rom_polygon_pt , 
polygon->plane_number ) ) { 
points_found = TRUE; 

break; 

] 

] 

} 

else { 

for(i = 1; 

i <= second_polygon_start->number_nodes && 
i <- polygon- >polygon_start->number_nodes ; 
i++ ) ( 

to_polygon_pt = plus_tangent ( second_polygon_start , 

shortest->last_node->pt) ; 

f rom_polygon_pt = minus_tangent ( polygon->polygon_start , to_polygon_pt ) 
if ( ! cross_polygon_node_on ( start_2d, to_polygon__pt , f rom__polygon_pt , 
polygon->plane_number ) ) { 
points_found = TRUE; 

break ; 

) 

) 

) 

] 

if (points_found) 

tangent_path_list = make_tangent_list ( to_polygon_pt , from_polygon_pt ) ; 
return ( tangent_path_list ) ; 



a path 
point 



*make_tangent_list ( to_point , from_point ) 
to_point, from_point; 



{ 

a_path 



♦list ; 



NEW_NODES ( list ) ; 

NEW__NODES ( list->next_node ) ; 
list->next_node->next_node = NULL; 
list->next_node->next_after_node = list; 
list->next_after_node = NULL; 



list->pt = assign_point_values ( to_point) ; 

list- >next_node->pt = assign_point_values ( f rom_point ) ; 

return (list); 
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/ 



PLUS TANGENT 

- - 0O0 

This functions finds and returns the node which with the inputted point 
forms the plus common tangent. It calls the the order function. Sign is 
a macro and is defined in 3d_tan.h and returns an integer corresponding 
to the value returned by order. The function returns the point found to 
the calling function*/ 

point plus_tangent ( start , pi) 

obstacle_plane *start; 
point pi; 

{ 

obstacle_plane *next = start->ccw, *prev = start->cw, *current = start; 

int count; 

point pt; 

double dirl, dir2; 

for (count = 1; count <= start- >number_nodes ; count++) { 

if (current->pt .x == pl.x && current->pt . y == pl.y) ( 
current = current->ccw; 
break ; 

) 

if (sign(order(pl, current, next)) == 1) { 
current = next; 
next = next->ccw; 

} 

else ( 

if (sign(order (pi, current, prev) ) == 1) { 

current = prev; 
prev = prev->cw; 

1 

else 

break; 

J 

} 

if (atan2 ( pi . y - current->pt . y , pl.x - current- >pt . x ) == 

atan2(pl.y - current- >ccw->pt . y , pl.x - current->ccw- >pt . x ) ) ( 
if (distance(pl, current->pt) < distance(pl, current- >ccw->pt) ) 
return (current->pt) ; 
else 

return ( current- >ccw- >pt ) ; 

J 

if (atan2 (pi . y - current->pt . y , pl.x - current- >pt . x ) == 

atan2(pl.y - current- >cw- >pt . y, pl.x - current- > cw- >pt . x )) { 
if (distance(pl, current->pt) < distance(pl, current- >cw- >pt ) ) 
return ( current->pt ) ; 
else 

return ( current->cw->pt) ; 

} 

return ( current->pt ) ; 

} 
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/ 



MINUS TANGENT 



o 0o 

finds and returns the node in the polygon which, with the inputted point 
forms the minus common tangent. It calls the the order function. Sign 
is a macro and is defined in 3d_tan . h and returns an integer corresponding 
to the value returned by order. The function returns the point found to 
the calling function*/ 

point minus_tangent ( start , pi) 

obstacle_plane *start; 
point pi; 

{ 

obstacle_plane *next = start->cw, *prev = start->ccw, ^current = start; 
int count; 

point pt; 

for (count = 1; count <= start- >number_nodes ; count++) { 

if (current->pt . x == pl.x && current->pt . y == pl.y) ( 
current = current->cw; 
break; 

) 

if (sign(order (pi, current, next)) == -1) ( 
prev = current; 
current = next; 
next = next->cw; 

3 

else { 

if ( sign(order ( pi, current, prev)) == -1) { 
next = current; 
current = prev; 
prev = prev->ccw; 

) 

else 
break ; 

3 

3 

if ( a tan2 ( pi . y - current- >pt . y, pl.x - current->pt .x) == 

atan2(pl.y - current- >ccw->pt . y, pl.x - current ->ccw- >pt . x )) { 
if ( distance( pi , current->pt) < distance(pl, current->ccw->pt ) ) 
return ( current->pt ) ; 
else 

return ( current - >ccw- >pt ) ; 

3 

if (atan2 ( pi . y - current->pt . y , pl.x - current- >pt . x) == 

atan2(pl.y - current- >cw- >pt . y , pl.x - current->cw->pt . x ) ) { 
if (distance(pl, current->pt) < distance(pl, current->cw->pt ) ) 
return ( current->pt ) ; 
else 

return ( current->cw->pt ) ; 

3 

return ( current->pt ) ; 

3 
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APPENDIX J 



This appendix contains the source code of vertical. c. This code corresponds to 
those functions used to find the vertical path over a polyhedron. 

# include "3d_tan.h" 

#include "plot.h" 

tfdefine LOOP 



/* 



FIND VERTICAL NODES 
000 



This function finds the appropriate vertical points formed by the inter- 
section of the vertical plane and each polyhedron. The function cycles 
through the polyhedron list checking each polyhedron and finding all 
intersection points for that polyhedron. It then determines the two 
highest points and checks checks if the intersection points are between 
the expanded point and the goal. If the points are, the function calls 
new_vertical_new_list , which places one of the two or both of the points 
into a vertical list (link list). The function then checks the next 
polyhedron, finding the appropriate intersection points and calls the 
new_vertical_new_list which replaces the points in the vertical list if 
they satisfy the requirments of the heuristics in that function. It 
recieves the vertical list from new_vertical_new_list and passes it to 
the calling function upon completion.*/ 



vertical_path *f ind_vertical_nodes (expand_pt, end_pt) 

point expand_pt, end_pt; /* expand_pt is the point from 

* which the vertical list is 

* formed, end_point is the 

* ending point that is used 

* to form the vertical list */ 



point ptl, pt2, temp; 

obstacle_list *current_polyhedron ; 
polyhedron_obstacle *face; 
polyhedron_obstacle_plane *current_node; 

int count, boolean = FALSE, two_points = FALSE; 

vertical_path *vertical_l ist = NULL; 

vertical_n_perpendicular_plane( expand_pt , end_pt) ; 

for (current_polyhedron = polyhedron_list ; 
current_polyhedron != NULL; 

current_polyhedron = current_polyhedron- >next_polygon , 
two_points = FALSE, boolean = FALSE) { 

for (face = current_polyhedron->obstacle; 

face != NULL; face = face->next_face) { 

for ( current_node = f ace- >face_nodes , count = 1; 
count <= face->face_nodes->number_nodes ; 
current_node = current_node->ccw, count++) ( 
if (two_points ~ FALSE) [ 
if (boolean == FALSE) ( 

ptl = intersection_point (current_node->pt , 

current_node->ccw->pt, vertical) ; 
if (ptl.x != NO_INTERSECTION ) { 

boolean = TRUE; 
continue; 
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} 

} 

else { 

pt2 = intersection_point (current_node->pt , 

current_node->ccw->pt, vertical) ; 
if ( pt2 . x != NO_INTERSECTION) { 
two_points = TRUE; 
if (ptl.z > pt2 . z ) { 

temp = swap(ptl); 
ptl = swap( pt2 ) ; 
pt2 = swap (temp); 

} 

} 

} 

) 

else { 

temp = intersection_point ( current_node->pt , 

current_node->ccw->pt, vertical) ; 
if ( temp . x ! = NO_I INTERSECTION) { 

if (temp.x == ptl.x && temp.y == ptl.y && temp . z == ptl.z) { 
if (temp.x == pt2.x && temp.y == pt2.y && temp . z == pt2.z) { 
continue ; 

} 

} 

if (temp.z < ptl.z) { 
pt2 = swap(ptl); 
ptl = swap (temp); 

} 

else { 

if (temp.z < pt2 . z ) 
pt2 = swap(temp); 

} 

} 

} 

} 

} 

if (ptl.x != NO_INTERSECTION && boundry_check ( boundry , ptl ) && 
boundry_check ( boundry , pt2) && 

(ptl.x != end_pt.x | | ptl.y != end_pt.y) && (ptl.x != expand_pt.x | | 
ptl.y ! = expand_pt.y) && 

(pt2.x != expand_pt.x | | pt2.y != expand_pt . y ) ) ( /*the or checks are 

to ensure the expand or end point are not included as 
in the case of a face edge*/ 

if (vertical_node_direction_heuristic(end_pt , expand_pt, ptl) && 
vertical_node_direction_heuristic(end_pt , expand_pt, pt2)) 



vertical_list = vertical_node_list_f rom_pt( vertical_list , ptl, pt2, 

expand_pt, current_polyhedron->polyhedron_number ) ; 



} 

} 

return ( vertical_list ) ; 



point 

point 

{ 

point 



swap( pt ) 
pt; 

ptl; 



ptl .x = pt . x; 
ptl.y = pt.y; 
ptl.z = pt.z; 
return (ptl); 
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NEW VERTICAL NODE LIST FROM POINT 



This function uses the two vertical points passed to it from find_verti- 
cal_nodes, which represent the vertical nodes from a polyhedron, and 
places them in eucledian distance order in a vertical_list . It returns 
to the calling function this vertical_list . 

*/ 

vertical_path *vertical_node_list_f rom_pt (vertical_list , ptl , pt2, pt, poly_num) 
vertical_path *vertical_list ; 



point 
int 

double 
a_pa th 



ptl, pt2 , pt; 
poly_num; 

ptl_distance, pt2_distance, pt_dist; 
♦current, *previous, *temp; 



ptl_distance = distance ( ptl , pt); 
pt2_distance = distance ( pt2 , pt); 

NEW_NODES(temp) ; 
temp->next_node = NULL; 

if (ptl_distance == pt2_distance) { 

temp->pt = assign_point_values (ptl ) ; 
temp->polygon_number = poly_num; 

) 

if (ptl_distance 1= pt2_distance ) { 

NEW_NODES ( temp - > next_node ) ; 
temp->next_node->next_node = NULL; 
if (ptl_distance > pt2_distance) { 

temp->pt = assign_point_values (ptl ) ; 

temp- >next_node->pt = ass ign_point_values ( pt2 ) ; 

} 

else{ 

temp->pt = assign_point_values (pt2 ) ; 
temp->next_node->pt = ass ign_point_values ( ptl ) ; 

] 

temp- >polygon_number = poly_num; 
temp->next_node->polygon_number = poly_num; 

) 

if (vertical_l ist == NULL){ 

NEW_VERTICAL(vertical_list) ; 
vertical_list->next_node - temp; 

] 

else { 

for (current = vertical_list->next_node, previous * NULL; 
current != NULL | | temp == NULL; 
previous = current->next_node, 
current = current- >next_node->next_node ) ( 

pt_dist = distance(pt, current->pt ) ; 

if (ptl_distance > pt_dist || pt2_distance > pt_dist){ 
temp->next_node->next_node = current; 
if (previous == NULL) { 

vertical_l ist->next_node = temp; 
break; 

] 

else 

previous- >next_node = temp; 

) 

if (current->next_node == NULL){ /*considers only one node in list 

and temp nodes come after it*/ 
current->next_node = temp; 
break; 

] 

] 

if (temp 1= NULL && previous != NULL) 
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previous - >next_node = temp; 

} 

vertical_list->next_node->next_af ter_node = NULL; 
return (vert ical_l is t) ; 



/*" 

INSERT VERTICAL LIST 

000 

This function takes the vertical list and inserts the list as a valid 
path in the start_paths list. 

V 

paths * insert_verticle_list (startpaths , current_path, vertical_list , tan4vert, 

end_pt, polygon4vert ) 

paths *start_paths, *current_path; 

vertical_path *vertical_list ; 
int tan4vert; 

point end_pt; 

{ 

paths *temp; 
a_path ^current; 
double cost = 0; 

NEW_PATH ( temp ) ; 

temp->last_node = vertical_list- >next_node ; 
current - temp- >last_node; 
current->next_af ter_node = NULL; 
temp- >last_node->next_af ter_node = NULL; 
free( vertical list ) ; 



/♦connect the double link list and calculate the distance from the 
first node to the last node*/ 
for( ; LOOP ; current = current- >next_node) ( 
if (current->next_node != NULL) [ 

current->next_node->next_af ter_node = current; 

cost += distance (current- >pt/ current->next_node- >pt ) ; 

] 

else 

break; 

] 

temp- >polygon4 vert = polygon4vert; 
temp->polygon4vert_PTR = NULL; 
temp->cost = cost; 

temp->estimated_cost = distance( temp->last_node->pt, goal); 
temp- >total_cost = cost + temp->estimated_cost; 
temp- >type_tan gent = VERTICAL; 

temp->ray_direction - atan2 ( ( temp->last_node->pt . y - 

temp - > las t_node - > next_node - > pt . y ) , 

( temp->last_node->pt . x - 
temp->last_node->next_node->pt.x) ) ; 

temp- >plane_number = 0; 
temp->when_path_found = paths_found++ ; 
temp- >able2expand = TRUE; 

temp- >polygon4vert_PTR = input_data4polyhed4vert ( temp->polygon4vert_PTR, 

end_pt, tan4vert); 



temp->next_path = current_path->next_path; 
if (start_paths == NULL) 
start_paths = temp; 
else{ 

temp- >next_path = current_path- >next_path; 
current_path- >next_path = temp; 
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# i fdef DOS 

if ( plot2screen ) { 

for (current = temp- >las t_node; 
current->next_node 1= NULL; 
current = current->next_node) 

draw_line_segment (current- >next_node->pt , current->pt, YELLOW, 

REGULAR) ; 

) 

#endif 

return ( start_paths ) ; 

} 



/* 



VERTICAL NODE DIRECTION HEURISTIC 
- OOO 



This function determines if a point lies in the same xy direction from a 
start point as the direction of the line from the start_pt to the goal. 
The start_pt is not the same as the global start_point but just a local 
variable and could any valid point in the valid possible path list 

V 

int vertical_node_direction_heur istic (goal, expand_pt, pt ) 

point goal, expand_pt, pt; 

{ 

double x, y, a; 



if(goal.x == expand_pt.x || pt.x == expand_pt.x) 
expand_pt.x += .0000001; 
if(goal.x == pt.x) 

goal = increase_goal_x (goal , expand_pt); 



x = atan2(goal.y - expand_pt.y, goal.x - expand_pt . x ) ; 
y = atan2(goal.y - pt.y, goal.x - pt.x); 
a = x - y; 



if (ABS(a) <= QPI ) 
return ( 1) ; 
else 

return (0); 



/* 

INCREASE GOAL X 

OOO 

This function is called if its determined that the increase and pt2 points 
have the same value for the x coordinate. if so then it adds of subtracts 
.000001 to the x component goal so that the new distance from the two 
points is greater than the original distance. If not done, atan2 is 
undetermined since x - x == 0 

V 

point increase_goal_x ( increase, pt2) 
point increase, pt2; 

{ 

double orginal_distance; 

orginal_distance = distance( increase, pt2); 
increase. x += .0000001; 

if (distance( increase, pt2) < orginal__distance) 
increase. x -= .0000002; 

return ( increase) ; 



} 



139 



/* 

UP AND OVER PATHS 

- - OOO - 

This function calls two other functions. The first finds the verti- 
cal nodes of a possible path and places them in a list labled verti- 
cal_list. The second function takes the vertical_list , places it 
in the possible paths list and updates the header list of the path. 

V 

paths *up_and_over ( start_paths , current_path, exp_pt, end_pt, beginpt, mode, 
polygon4vert ) 

paths *start_paths , *current_path; 

point exp_pt, end_pt, beginpt; 

int mode, polygon4vert ; 

{ 

vertical_path *vertical_list - NULL; 
a_path ♦current; 

vertical_list = f ind_vertical_nodes (exp_pt, end_pt); 
if ( vertical_list 1= NULL) { 

if (beginpt. x == exp_pt.x && beginpt. y == exp_pt.y && 
beginpt. z == exp_pt.z){ 
for(current = vertical_list->next_node; 
current- >next_node !- NULL; 
current = current- >next_node) ; 

NEW_NODES ( current->next_node ) ; 

current - > next_node- > next_node = NULL; 

current->next_node->pt = assign_point_values ( exp_pt ) ; 

] 

vertical_list = trim_not_needed_nodes ( vertical_list ) ; 
start_paths = insert_verticle_list ( start_paths , current_path, 

vertical_lis t, mode, end_pt, 
polygon4vert ) ; 

] 

if (VERTICAL ! = mode){] 
return ( star t_paths ) ; 

} 

/* 

CONTINUE VERTICAL PATH 

OOO 

This function finds the one node/tangent which can extend the verti- 
cal path by one node on the perpendicular plane formed by the verti- 
cal node and the goal. There is only one node to consider since it 
falls on the line segment of the polyhedron used to find the verti- 
node 

V 

paths ♦continue_vertical_path ( shortest , start2d) 
paths ♦shortest; 
polygon_list *start2d; 

{ 

polygon_list ♦polygon = start2d; 

point ptl, pt2; 

a_path *temp; 

int tan, found = FALSE; 

double ray_direction, difference; 

for (; polygon != NULL; polygon - polygon - >next_polygon ) [ 
if ( polygon->plane_number != shortest->plane_number ) 
continue; 

if ( polygon->polyhedron_number != shortest->polygon4vert ) 
continue; 
else 
break; 

) 
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if(l == shortest->polygon4vert_PTR-> type_tangent ) 

ptl = plus_tangent ( polygon -> poly gon_s tart, shortest- >last_node- >pt ) ; 
else 

ptl = minus_tangent (polygon->polygon_start , shortest-> last_node->pt ) ; 

ray_direction = atan2(ptl.y - shortest-> last_node- >pt . y , 

ptl.x - shortest->last_node- >pt . x ) ; 

/*used le-3 instead of 0 because of the precision of DOS machine. No 
points used evaluated true and the point which should have evalueated 
true, had a difference of le-10*/ 



difference = shortest- >ray_direction - ray_direction; 



/*find the up_around and up paths before continuing on*/ 



if (ABS(dif ference) < le-3)( 

NEW_NODES ( temp ) ; 

temp->pt = assign_point_ values ( ptl ) ; 
temp->polygon_number = polygon ->polygon_number ; 
temp->next_node = shortest->last_node; 
shortest->last_node = temp; 

if ( shortest- >poly go n4 vert PTR->type tangent) tan = PLUS; 
else tan = MINUS; 

shortest = update ( shortest , tan); 
found = TRUE; 



) 

else { 

shortest = recalculate_vertical_path( ptl , shortest, 

( shortest- >polygon4vert_PTR->type_tangent 



MINUS) ) ; 

shortest->last_node- >polygon_number = polygon ->polygon_number; 
found - TRUE; 



] 



9PLUS 



if ( ! found) 

shortest->able2expand = NO; 

# if def DOS 
else{ 

if (plot2screen) 

draw_line_segment( shortest- > las t_node- >next_node->pt, 
shortest->last_node->pt , YELLOW, REGULAR); 



) 

#endif 

return ( shortest ) ; 

] 



/*- - 

UP AND OVER FROM GOAL 

000 - 

This function finds all the vertical partial paths from the goal 
extending to the start. This working backwards from the goal allows 
me to find the valid paths which go around one or more obstacles and 
over the rest of them. 

V 

paths *up_and_over_f rom_goal ( paths *start_paths , paths *current_path, 

point goal, polygon_list *start2d) 

/* paths *start_paths, *current_path; 
point goal; 

polygon_list *start2d; 

V 

( 

polygon_list *current_polygon ; 
point ptl, pt2; 
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int poly4pt; 



for ( current_polygon = start_2d; 
currentpolygon != NULL; 

current_polygon = current_polygon- >next_polygon ) { 

ptl = plus_tangent ( current_polygon->polygon_start , goal); 
pt2 = minus_tangent (current_polygon->polygon_start, goal); 

if (cross_polygon (start_2d, goal, ptl) && 
direct ion_heuristic_from_start( goal, ptl) && 
boundry_check(boundry , ptl)) { 

start_paths = up_and_over ( start_paths , current_path , goal, 

ptl, goal, PLUS, current_polygon- >polyhedron_number ) ; 
if (current_path == NULL) current_path = start_paths; 
else current_path = current_path->next_path; 
current_path-> forward = NO; 

} 

if ( cross_polygon ( start_2d, goal, pt2) && 
direct ion_heuristic_f rom_start ( goal , pt2) && 
boundry_check (boundry , pt2)) [ 

start_paths = up_and_over ( start_paths , current_path, goal, 
pt2 , goal, MINUS); 

if (current_path == NULL) current_path = start_paths; 
else current_path = current_path-> next_path; 
current_path->forward = NO; 

current_path->polygon4vert = current_polygon->polyhedron_number ; 

] 

} 

return ( start_paths ) ; 



paths *recalculate_vertical_path( pt , path, tan) 
point pt; 
paths *path; 
int tan; 

{ 

vertical_path *vertical; 
a_path *node = path- >last_node, 

♦current, *previous; 
double cost = 0; 
int count, number_nodes ; 

/♦find the last node (or beginning pt of path)*/ 

for( ; node->next_node != NULL; node = node->next_node) ; 

/♦disconnect the last node from the path*/ 
node->next_af ter_node- > next_node = NULL; 
node->next_af ter_node = NULL; 

/♦delete the path nodes of the shortest_path 
while counting number of nodes*/ 

for (current = path->last_node, previous = current, number^nodes = 0; 
current != NULL; 

previous = current, current= current- >next_node, number_nodes++ ) 
free ( previous ) ; 

free ( previous ) ; 

/♦find the new vertical list*/ 

vertical = f ind_vertical_nodes ( node- >pt , pt); 

/♦finds the end of the vertical list then working backwards up the 
list, places the same number of nodes in the list*/ 
for(current = vertical->next_node; current- >next_node != NULL; 
current = current- >next_node) 
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current->next_node->next_after_node = current; 

for (count = 1; count < number_nodes ; 

count++, current= current- >next_af ter_node) ; 
vertical->next_node = current; 

/♦frees up the unused portion of the list*/ 
for (current = current- >next_after_node; current != NULL; 
current = current->next_after_node) 

free( current ) ; 

path->last_node = vertical->next_node; 
free(vertical) ; 

path - insert_pt_into_list ( path, pt); 
path->last_node->next_af ter_node - NULL; 

for(current = path->last_node; 
current- >next_node != NULL; 
current = current->next_node) { 
current->next_node->next_after_node = current; 
cost += distance( current->pt , current- >next_node->pt ) ; 

} 

current- >next_node = node; 
node->next_af ter_node = current; 
cost + = distance(current->pt, node->pt); 
if ( path- >forward == YES) 

path->estimated_cost = distance ( node- >pt , goal); 
else 

path->estimated_cost = distance( node->pt , start_point ) ; 

path->total_cost = cost + path- >estimated_cost ; 
path->cost = cost; 

path->ray_direct ion = atan2 (path->last_node->pt . y - 

path -> las t_node->next_node- >pt . y , 
path- >last_node->pt . x - 
path->last_node->next_node->pt . x) ; 

path->type_tangent = tan; 
return (path) ; 

} 

paths *insert_pt_into_list (path, pt) 
paths *path; 
point pt; 

[ 

a_path *temp = NEW_NODES ( temp) ; 
temp->pt = assign_point_values ( pt ) ; 
temp->next_af ter_node = NULL; 
temp->next_node = path->last_node; 
path->last_node - temp; 

return (path) ; 

} 

polyhed4 * input_data4polyhed4vert ( ptr , end_pt, mode) 
polyhed4 *ptr; 
point end_pt; 
int mode; 

{ 

if ( NULL == ptr) 

ptr = (polyhed4 *) malloc ( sizeof ( polyhed4 ) ) ; 

ptr->x = end_pt.x; 
ptr->y = end_pt.y; 
ptr- >type_tangent = mode; 

return ( ptr ) ; 

} 
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vertical_path * trim_not_needed_nodes ( 1 ist ) 
vertical_path *list; 

I 

a_path *nodel, *node2, *node3, *node4; 

for(nodel = list->next_node; 

NULL ! = nodel ; 

nodel = nodel- >next_node ) { 

if (NULL — nodel ->next_node->next_node) break; 

for(node2 = nodel ->next_node- >next_node; 

NULL ! = node 2 ; 

node2 = node2- >next_node ) [ 

if ( intersect_polyhedron(nodel->pt, node2->pt)) continue; 
else { 

for(node3 = nodel->next_node, node4 = node3; 
node 2 ! = node3; 

node4 = node3, node3 = node3->next_node , f ree ( node4 ) ) 

nodel- >next_node = node2; 
node2- >next_after_node = nodel; 

} 

} 

} 

return( list ) ; 

} 



144 



APPENDIX K 



This appendix contains the source code associated with visibile.c. 

#include "3d_tan.h" 
tfinclude "plot.h" 

#def ine LOOPFOREVER 1 



/* oOo 

VISIBILITY 

o 0o 

This function determines whether or no the last point in the partial path is 
visible to the goal or not. The function considers whether a point on the 
backside of a polygon is visible to the goal by first finding the tangents to 
the goal with the polygon and traces the path around to the expansion point. 

A point is valid in this case if no obstacles lie between the goal and the 
tangent point 
*/ 

a_path ^visibility (expand_point / goal_point, start, two__d_list) 
point expand_point , goal_point, start; 
polygonal is t *two_d_list; 

{ 

polygon_list ^polygon; 
point f rom_polygon_pt , goal; 
a_path *temp, *templ, *goal_list; 
obstacle_plane *node; 
int tangent = 1; 

goal = determine_end_point ( start , goal_point) ; 
if ( ! intersect_polyhedron ( expand_point , goal ) ) { 

NEW_NODES ( goa 1_1 is t ) ; 
goal_list- >next_node = NULL; 
goal_list- >pt = assign_point_values (goal ) ; 
return ( goal_list ) ; 

} 

if ( shortest_path->type_tangent == VERTICAL) 
return ( NULL) ; 
else{ 

/*find polygon which last node of shortest path lies on*/ 
for (polygon = two_d_list; 

polygon- >polygon_number != shortest_path->last_node->polygon_number | 
polygon- >plane_number != shortest_path->plane_number ; 
polygon - polygon- >next_polygon ) ; 

while ( tangent <= 2){ 
if(l == tangent) 

/♦finds the plus tangent from the goal to the polygon last node of 
shortest path lies on*/ 

from_polygon_pt = plus_tangent ( polygon->polygon_start, goal); 
else 

/♦check the minus tangent*/ 

f rom_polygon_pt = minus_tangent (polygon->polygon_start, goal); 

/♦checks for intersection, if there is return a zero*/ 
if ( intersect_polyhedron ( f rom_polygon_pt , goal ) ) 
return ( NULL) ; 

elsef /*two points are visible*/ 

if (temp == NULL){ 

NEW_NODES ( temp ) ; 
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temp->next_node = NULL; 
temp->next_af ter_node - NULL; 

} 

temp->pt = assign_point_values ( from_polygon_pt ) ; 
tempi = temp; 

/♦finds the node on the polygon which the f rom_polygon_pt lies on*/ 
for (node = polygon- >polygon_start; 
node->pt.x != f rom_polygon_pt . x 
node->pt.y != f rom_polygon_pt . y 
node->pt.z != f rom_polygon_pt . z ; 
node = node->ccw) ; 

while (LOOPFOREVER) { 
if(l == tangent){ 

/♦checks to see if the next counter clockwise node is the 
expansion node*/ 

if ( node- >ccw->pt . x == shortest_path->last_node->pt . x && 
node- >ccw->pt . y -= shortest_path- >last_node->pt . y && 
node- >ccw- >pt . z == shortest_path- >last_node->pt . z ) 
break; 
elsef 

if ( tempi ->next_node -- NULL) { 

NEW_NODES ( tempi- >next_node) ; 

tempi- >next_node->next_after_node = tempi; 

tempi- >next_node->next_node = NULL; 

} 

tempi = tempi ->next_node; 

templ->pt - assign_point_values ( node->ccw- >pt ) ; 

) 

node = node->ccw; 

) 

else{ /*tangent == 2*/ 

/♦checks to see if the next counter clockwise node is the 
expansion node*/ 

if ( node- >cw- >pt . x -= shortest_path->last_node- >pt . x && 
node->cw->pt .y == shortest_path->last_node- >pt . y && 
node- >cw->pt . z == shortest_path->last_node->pt . z ) [ 
if ( tempi ->next_node != NULL){ 
for (tempi = tempi- >next_node, 

tempi- >next_af ter_node->next_node = NULL, 
tempi ->next_after_node = NULL, 
tempi - > next_node ; 
tempi- >next__node != NULL; 
tempi = tempi- >next_node) 
free ( tempi ->next_after_node) ; 
if ( tempi ->next_after_node != NULL) 
f ree ( tempi ->next_af ter_node) ; 
free( tempi ) ; 

} 

for (tempi = temp; temp->next_node != NULL; temp = temp->next_node) ; 
break ; 

1 

else[ 

if ( tempi ->next_node == NULL) ( 

NEW_NODES ( tempi - > nex t_node ) ; 

tempi- >next_node->next_af ter_node - tempi; 

tempi- >next_node->next_node = NULL; 

) 

tempi = tempi- > nex t_node; 

templ->pt = assign_point_values (node->cw->pt ) ; 

} 

node = node->cw; 

) 

) 

if (direction_heur istic (shortest_path, templ->pt))[ 
goal_list = temp; 
return ( goal_list) ; 
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} 

] 

tangent += 1; 

} 

} 

for(templ = temp->next_node ; tempi ->next_node != NULL; tempi = tempi - >next_node ) 
free( templ->next_af ter_node) ; 
free ( temp- >next_after_node) ; 
free (temp) ; 
return ( NULL) ; 



point determine_end_point( start / goal) 
point start, goal; 

I 

if ( short est_path-> forward ) return (goal ) ; 
else return ( start ) ; 

} 
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APPENDIX L 



This appendix contains the two header files (3d_tan.h and plot.h). 

#include <math.h> 

#include <stdio.h> 



/* 



3D TANGENT HEADER FILE ( 3D_TAN . H) 
O 0o 



V 



#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 

#def ine 

#def ine 

#def ine 

#def ine 

#def ine 

#def ine 

#def ine 

#def ine 

#def ine 

#def ine 

#def ine 



READONLY 

WRITEONLY 

TRUE 

FALSE 

NO 

YES 



" r 
"w" 



NO_I NTERSECTION 
sqr(x) 

F(X,Y,Z) 

VERTICAL 
NONE 2 

PLUS 1 

MINUS 0 

GOAL_PT 1 

NOT_GOAL_PT 0 
sign ( a ) (((a) 

ABS(x) (((x) 

TOTAL_COST 
FOREVER 
QPI 
HPI 
PI 
DPI 

NEW_OBSTACLE(x) 



999999.9990 
(x * x) 

( ( pl_eq . x* temp_pt . x ) 
2 



+ ( pl_eq . z*temp_pt . z ) )/pl_eq . y 



< 0 ) ? -1 : 

< 0) ? -x : 
current_path->cost 
1 

0.785398230785398 



(a) 

(x) 



0 ) 

0 ) 



+ current_path->estimated_cost 



5707963257949 
141593123141593 
283186246283186 
( (x = (obstacle *) malloc 

(sizeof (obstacle) ) )==NULL?exit ( 1 ) : 1 ) 

((x = (obstacle_plane *) malloc 

(sizeof ( obstacle_plane ) ) )==NULL?exit ( 1 ) : 1 ) 

((x = (obstacle_list *) malloc 

(sizeof (obstacle_list) ) ) -= NULL?exit ( 1 ) : 1 ) 

( (x = ( polyhedron_obstacle *) malloc 

( sizeof (polyhedron_obstacle) ) ) == NULL?exit ( 1 ) : 1 ) 

((x = ( polyhedron_obstacle_plane *) malloc 
( sizeof (polyhedron_obstacle_plane) ) ) == NULL?exit ( 1 ) : 1 ) 
((x = ( verticalpath *) malloc 

(sizeof (vertical_path) ) ) -= NULL?exit ( 1 ) : 1 ) 

((x = (goals *) malloc 

(sizeof (goals )) ) == NULL?exit ( 1 ) : 1) 

((x = (paths *) malloc 

( s izeof (paths )) ) == NULL?exit ( 1 ) : 1) 

((x = (a_path *) malloc 

(sizeof (a_path) ) ) == NULL?exit ( 1 ) : 1 ) 

((x = ( polygon_list *) malloc 

( sizeof (polygon_list) ) ) «= NULL?exit ( 1 ) : 1 ) 

((x = ( active_nodes *) malloc 
( sizeof (active_nodes )) ) =s = NULL?exit ( 1 ) : 1 ) 

POI NT_VALUES &x_coord, &y_coord, &z_coord 



NEW_PLANE (x ) 
NEW_N0DE(x) 
NEW_POLYHEDRON(x) 
NEW_POLY_PLANE ( x ) 
NEW_VERTICAL(X) 
NEW_GOAL(X) 
NEW_PATH ( x ) 
NEW__NODES (x ) 
NEW_P0LYG0N(x) 

N EW_ACT I VE_NODE ( x ) 
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# ifdef MAIN 

# define EXTERN 

# define INIT(Value) = Value 
#else 

# define EXTERN extern 

# define INIT(Value) 

#endif 



typedef struct polyhedron_list [ 
struct polyhedron *obstacle; 
int number_f aces ; 

int polyhedron_number ; 

struct polyhedron_list *next_polygon ; 
} obstacle_list; 



EXTERN obstacle_list *polyhedron_lis t INIT ( NULL) ; 

EXTERN obstacle_list *intersected_polyhedron INIT (NULL); 



typedef struct polyhedron [ 

struct polyhedron_node *face_nodes; 
int number_nodes ; 

struct polyhedron *next_face; 

} polyhedronobstacle, polyhedronf ace; 



typedef struct 
double 
double 
double 
double 
double 
double 



posture { 
x; 

y; 

z; 

phi; 

t; 

psi; 

POSTURE; 



typedef struct 
double 
double 
double 



x_y_z { 
x; 

y; 

z; 

point; 



typedef struct polyhedron_node { 
struct x_y_z pt; 

int number_nodes ; 

struct polyhedron_node *cw; 
struct polyhedron_node *ccw; 

) polyhedron_obstacle_plane / polyhedron_nodes , obstacle_plane 



EXTERN point start_point, goal, intersection; 



typedef struct 
double 
double 
double 
double 



plane_equation { 
x; 

y; 

z; 

d; 

equation; 



EXTERN equation vertical, perpendicular; 

typedef struct two_d_polygon_lis t [ 
obstacle_plane *polygon_start ; 
struct two_d_polygon_list *next_polygon; 
int plane_number ; 

int po 1 y go n_n umber ; 

int polyhedron_n umber ; 

} polygon_list ; 

EXTERN polygon_list *start_2d INIT ( NULL) ; 
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/*typedef struct polygon_node { 
struct x_y_z pt; 

int number_nodes ; 

struct polygon_node *ccw; 
struct polygon_node *cw; 

} obstacle_plane; */ 

typedef struct 1 ist_of_active_nodes ( 

point pt; 

double distance; 

struct 1 ist_of_active_nodes *next_active_node; 
int when_path_found; 

} active_nodes ; 



EXTERN active_nodes *head_active_nodes INIT ( NULL) ; 



typedef struct lis t_of_paths { 

struct 1 ist_of_paths *next_path; 
struct possible_paths *last_node; 
cost; 

estimated_cost ; 
total_cost ; 
ray_direction ; 
type_tangent; 
plane_number ; 
when_path_found; 
able2expand; 
forward; 
polygon4vert ; 
*polygon4 v ert_PTR; 
paths ; 



double 
double 
double 
double 
int 
int 
int 
int 
int 
int 

struct polyhed 



EXTERN path_and_node_list *head_path_node_list INIT (NULL) ; 

EXTERN paths *start_path INIT ( NULL) ; 

EXTERN paths *shortest_path INIT ( NULL) ; 

typedef struct possible_paths { 
point pt; 

struct poss ible_paths *next_node; 
struct possible_paths *next_af ter_node; 
int polygon_number ; 

) a_path, goals; 

typedef struct vertical{ 

double distance; 

struct possible_paths *next_node; 

) vert ical_path; 

EXTERN goals *goal_path INIT ( NULL ) ; 

typedef struct polyhed { 
double x, 

y; 

int type_tangent ; 

}polyhed4 ; 



typedef struct boundry_limits 



double 


xl; 


double 


x2 ; 


double 


yi; 


double 


y2; 


double 


zl; 


double 


z2; 

boundries ; 



EXTERN boundries boundry; 



{ 
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EXTERN char file [11] ; 

EXTERN char ^filename INIT(file); 

EXTERN int paths_found INIT(O); 

EXTERN int plane_number INIT(O); 

EXTERN int print2file INIT(O); 

EXTERN int horizontal INIT(O); 

EXTERN int plot2screen INIT(O); 

EXTERN int on_same_polygon INIT(O); 
EXTERN int number_polygons ; 

EXTERN int number_paths_f ound INIT(O); 
EXTERN double goal_direction INIT(O); 
EXTERN FILE *fpt2 INIT ( NULL) ; 

/* function declarations */ 



extern active_nodes * insert_nodes_into_active_node_l is t ( ) ; 

extern a_path *f ree_tangent_list ( ) ; 
extern a_path *make_tangent_list ( ) ; 
extern a_path *tangent(); 
extern a_path *vis ibil ity ( ) ; 

extern boundries ass ign_boundry_points ( ) ; 



extern double 
extern double 
extern double 
extern double 



function_one( ) ; 
f unction_two( ) ; 
norm( ) ; 
order ( ) ; 



extern int 
extern int 
extern int 
extern int 
extern int 
extern int 
extern int 
extern int 
extern int 
extern int 
extern int 
extern int 
extern int 



boundry_check ( ) ; 
check_active_node_list ( ) ; 
check_for_node_on_polygon ( ) ; 
cross_polygon ( ) ; 
cross_polygon_node_on ( ) ; 
count_polygon_nodes ( ) ; 
decision ( ) ; 

intersect_polyhedron ( ) ; 
lines_intersect ( ) ; 
point_on_line( ) ; 

vertical_node_direction_heuristic ( ) ; 
direction_heuristic_from_start ( ) ; 
direction_heur istic ( ) ; 



extern equation cross_product ( ) ; 
extern equation f ind_plane_equat ion ( ) ; 



extern double calculate_cost ( ) ; 
extern double distance(); 



char 



*malloc( ) ; 



extern obstacle_list *create_obstacle_list ( ) ; 
extern obstacle_plane *create_list ( ) ; 



extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 



*add_start_point_to_path( ) ; 
*create_path_n_nodes ( ) ; 
*connect_links_of_path_on_polygon( ) ; 
*connect_shortest_paths ( ) ; 

*cont inue_vertical_path( ) ; 
*delete_before_f inal ( ) ; 
*duplicate_entries ( ) ; 
*duplicate_paths ( ) ; 
*expand_paths_with_values ( ) ; 
*extend_path( ) ; 

*f ind_tangents_f rom_inner_node( ) ; 
*goal_reached_update( ) ; 
*insert_pt_into_list ( ) ; 
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extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 
extern paths 



*insert_verticle_list ( ) ; 

*insert_vertical_list_into_shortest_path( ) ; 
*mark_old_path_with_node_unexpandable( ) ; 
*mark_shortest_path_last_node( ) ; 
*up_and_over ( ) ; 

*up_and_over_f r°m_goal ( ) ; 

* u p_ar°und_ u p( ) ; 

*recalculate_vertical_path( ) ; 
*remove_shortest_path( ) ; 

*remove_paths ( ) ; 

*reversed_list ( ) ; 

*start_f inding_paths ( ) ; 

*update_current_path ( ) ; 

*update( ) ; 



extern point 
extern point 
extern point 
extern point 
extern point 
extern point 
extern point 
extern point 
extern point 
extern point 



assign_point_values ( ) ; 
determine_end_point ( ) ; 
f ind_shortest_path( ) ; 
increase_goal_x ( ) ; 
input_point ( ) ; 
intersection_point ( ) ; 
plus_tangent ( ) ; 
minus_tangent ( ) ; 

f ind_point_on_obstacle_plane_face( ) ; 
swap( ) ; 



extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 



polygon_list 
polygon_lis t 
polygon_list 
polygon_lis t 
polygon_list 
polygon_list 
polygon_lis t 
polygon_list 
polygon_list 



*add_on_new_plane_intersection ( ) ; 
*build_polygon ( ) ; 
*build_two_d_polygon_list ( ) ; 
*connect_links ( ) ; 

* f ree_polygon_list ( ) ; 

*make_list ( ) ; 

*remove_duplicate_nodes ( ) ; 
*remove_last_polygon ( ) ; 
*remove_planes ( ) ; 



extern polyhedron_f ace *f ind_polyhedron_f ace ( ) ; 



extern polyhedron_obstacle *create_f ace ( ) ; 
extern polyhedron_obstacle_plane *create_plane( ) ; 



extern polyhed4 *input_data4polyhed4vert ( ) ; 



extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 
extern void 



read_comment ( ) ; 
f ind_all_paths ( ) ; 
f ree_memory ( ) ; 
input_start_goal ( ) ; 
inside_boundries ( ) ; 
load_goal_path ( ) ; 

vertical_n_perpendicular_plane( ) ; 
print_intersection ( ) ; 
print_results ( ) ; 
print_2d_path( ) ; 
pr int_each_paths ( ) ; 
print_path( ) ; 
print_path_shortest ( ) ; 
print_polyhedron ( ) ; 
print_path_to_f ile( ) ; 



extern vertical_path 
extern vertical_path 
extern vertical_path 
extern vertical_path 



*f ind_vertical_nodes ( ) ; 

*new_vertical_new_list ( ) ; 

* tr im_not_needed_nodes ( ) ; 
*vertical_node_list_f rom_pt ( ); 
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PLOT . H 



#include <graphics.h> 
#include <dos . h> 




#def ine 


DOS 


1 


#def ine 


ESC 


Oxlb/* 


#def ine 


SCALEDX 


x / 48 


#def ine 


SCALEDY 


y / 34 


#def ine 


DOT 


1 


#def ine 


RED 


12 


#def ine 


YELLOW 


14 


#def ine 


REGULAR 


0 


#ifdef MAIN 





# define EXTERN 

# define INIT(Value) = Value 
#else 

# define EXTERN extern 

# define INIT(Value) 

#endif 



Define 



the escape key*/ 



/*used to make a dot with radius 



IV 



EXTERN 

EXTERN 

EXTERN 

EXTERN 

EXTERN 

EXTERN 

EXTERN 

EXTERN 

EXTERN 



int GraphDriver INIT(O);/* The Graphics device driver*/ 
int GraphMode INIT(O);/* The Graphics mode value*/ 

int MaxX, MaxY INIT(O);/* The maximum resolution of the screen */ 

int MaxColors INIT(O);/* The maximum # of colors available*/ 

int ErrorCode INIT(O);/* Reports any graphics errors*/ 

double AspectRatio INIT(O.O);/* Aspect ratio of a pixel on the screen*/ 

struct palettetype palette;/* Used to read palette info*/ 

int Expansion_lable_x INIT(O);/* Used to label the expansion point*/ 

int Expansion_lable_y INIT(O); 



extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 



void data_plot(); 

void Initialized ) ; 

void Pause( ) ; 

void MainWindow( ) ; 

void DrawBorder ( ) ; 

void StatusLine( ) ; 

void changetextstyle( ) ; 

void draw_boundry ( ) ; 

void draw_polygons ( ) ; 

void draw_shortest_path( ) ; 



extern void draw_line_segment ( ) ; 

extern void id_shortes t_path_for_expansion ( ) ; 
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APPENDIX M 



This first example is for 1 polyhedron and corresponds to the first example in 
Chapter IV. 

INITIAL PARTIAL PATHS 

PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 11.589, est.cost = 31.192, total = 42.781 

Plus Tangent, direction = 0.661 
(10.00, 20.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost = 31.811, total = 42.825 

Minus Tangent, direction = -0.588 
(10.00, 7.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 3 

PATH NUMBER 2 Path able to extend 

cost = 13.236, est.cost = 28.263, total = 41.499 

Minus Tangent, direction = 0.051 
(14.00, 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(1.00, 13.00, 5.00) 



THE GOAL HAS BEEN REACHED 

PATH FINDING ITERATION COUNT = 2 

PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 11.589, est.cost = 31.192, total = 42.781 

Plus Tangent, direction = 0.661 
(10.00, 20.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost = 31.811, total = 42.825 

Minus Tangent, direction = -0.588 
(10.00, 7.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 3 

PATH NUMBER 2 Path able to extend 

cost = 13.236, est.cost = 28.263, total = 41.499 



Minus Tangent, direction = 0.051 
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(14.00/ 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(1.00, 13.00, 5.00) 

SHORTEST PATH 

(14.00, 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(1.00, 13.00, 5.00) 

The total length of the shortest path is 41.499 



PATH FINDING ITERATION COUNT - 3 
PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 11.589, est.cost = 31.192, total = 42.781 

Plus Tangent, direction = 0.661 
(10.00, 20.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost = 31.811, total = 42.825 

Minus Tangent, direction = -0.588 
(10.00, 7.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 3 

PATH NUMBER 2 Path able to extend 

cost = 41.499, est.cost = 0.000, total - 41.499 



Minus Tangent, 
(40.00, 15.00, 
(40.00, 15.00, 
(14.00, 13.67, 
(10.00, 13.46, 
(1.00, 13.00, 
SHORTEST PATH 



direction 
14.00) to 

14.00) to 

3.00) to 
3.00) to 

5.00) 



0.051 



(40.00, 15.00, 14.00) to 
(40.00, 15.00, 14.00) to 
(14.00, 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(1.00, 13.00, 5.00) 

The total length of the shortest path is 41.499 



This example is the listing generated by the algorithm for the second example for 
a one polyhedral world in Chapter IV. 

INITIAL PARTIAL PATHS 

PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 11.589, est.cost = 31.192, total = 42.781 

Plus Tangent, direction = 0.661 
(10.00, 20.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost = 31.811, total = 42.825 
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Minus Tangent, direction = -0.588 
(10.00, 7.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 3 

PATH NUMBER 2 Path able to extend 

cost = 13.236, est.cost = 28.263, total - 41.499 

Minus Tangent, direction = 0.051 
(14.00, 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(1.00, 13.00, 5.00) 



THE GOAL HAS BEEN REACHED 

PATH FINDING ITERATION COUNT = 2 

PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 11.589, est.cost = 31.192, total = 42.781 

Plus Tangent, direction = 0.661 
(10.00, 20.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost - 31.811, total = 42.825 

Minus Tangent, direction = -0.588 
(10.00, 7.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 3 

PATH NUMBER 2 Path able to extend 

cost = 13.236, est.cost = 28.263, total = 41.499 

Minus Tangent, direction = 0.051 
(14.00, 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(1.00, 13.00, 5.00) 

SHORTEST PATH 

(14.00, 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(1.00, 13.00, 5.00) 

The total length of the shortest path is 41.499 



PATH FINDING ITERATION COUNT = 3 
PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 11.589, est.cost = 31.192, total = 42.781 

Plus Tangent, direction = 0.661 
(10.00, 20.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 11.014, est.cost = 31.811, total = 42.825 
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Minus Tangent, direction = -0.588 
(10.00, 7.00, 7.08) to 
(1.00, 13.00, 5.00) 



PATH 3 

PATH NUMBER 2 Path able to extend 

cost = 41.499, est.cost = 0.000, total = 41.499 



Minus Tangent, 
(40.00, 15.00, 
(40.00, 15.00, 
(14.00, 13.67, 
(10.00, 13.46, 
(1.00, 13.00, 
SHORTEST PATH 



direction 
14.00) to 

14.00) to 

3.00) to 
3.00) to 

5.00) 



0.051 



(40.00, 15.00, 14.00) to 
(40.00, 15.00, 14.00) to 
(14.00, 13.67, 3.00) to 
(10.00, 13.46, 3.00) to 
(1.00, 13.00, 5.00) 

The total length of the shortest path is 41.499 



This next example corresponds to the first example used in a two polyhedral world 
in Chapter IV. 

INITIAL PARTIAL PATHS 



PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 10.296, est.cost = 30.414, total = 40.709 

Plus Tangent, direction = 0.507 
(10.00, 20.00, 14.00) to 
(1.00, 15.00, 14.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 12.042, est.cost = 31.048, total = 43.090 

Minus Tangent, direction = -0.727 
(10.00, 7.00, 14.00) to 
(1.00, 15.00, 14.00) 



PATH 3 

PATH NUMBER 2 Path able to extend 

cost = 18.545, est.cost = 28.438, total = 46.983 

Minus Tangent, direction = 0.257 
(14.00, 18.42, 3.00) to 
(10.00, 17.37, 3.00) to 
(1.00, 15.00, 14.00) 



PATH 4 

PATH NUMBER 3 Path able to extend 

cost = 19.049, est.cost = 28.757, total = 47.806 

Minus Tangent, direction = -0.399 
(14.00, 9.53, 3.00) to 
(10.00, 11.21, 3.00) to 
(1.00, 15.00, 14.00) 
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PATH 5 

PATH NUMBER 4 Path able to extend 
cost = 28.213, est.cost = 19.416, total 



Minus Tangent, 
(24.00, 15.00, 
(20.00, 15.00, 
(14.00, 15.00, 
(10.00, 15.00, 
(1.00, 15.00, 



direction 
3.00) to 
3.00) to 
3.00) to 
3.00) to 
14.00) 



0.000 



PATH 6 

PATH NUMBER 5 Path able to extend 
cost = 24.216, est.cost = 23.640, total 

Minus Tangent, direction = -2.843 
(20.00, 8.85, 3.00) to 
(24.00, 10.08, 3.00) to 
(40.00, 15.00, 14.00) 



PATH 7 

PATH NUMBER 6 Path able to extend 
cost = 23.732, est.cost = 23.147, total 



Minus Tangent, 
(20.00, 18.85, 
(24.00, 18.08, 
(40.00, 15.00, 
SHORTEST PATH 



direction 
3.00) to 

3.00) to 

14.00) 



2.952 



(10.00, 20.00, 14.00) to 
(1.00, 15.00, 14.00) 

The total length of the shortest path is 



THE GOAL HAS BEEN REACHED 

PATH FINDING ITERATION COUNT = 2 

PATH 1 

PATH NUMBER 0 Path cannot be extended 
cost - 10.296, est.cost = 30.414, total 

Plus Tangent, direction = 0.507 
(10.00, 20.00, 14.00) to 
(1.00, 15.00, 14.00) 



PATH 2 

PATH NUMBER 10 Path able to extend 
cost = 28.613, est.cost = 21.541, total 

Minus Tangent, direction = -1.138 
(20.00, 7.00, 14.00) to 
(14.00, 20.00, 14.00) to 
(10.00, 20.00, 14.00) to 
(1.00, 15.00, 14.00) 



PATH 3 

PATH NUMBER 8 Path able to extend 
cost = 20.296, est.cost = 20.616, total 



47.629 



47.856 



46.879 



40.709 



40.709 



50.154 



40.911 



Plus Tangent, direction = 0.000 
(20.00, 20.00, 14.00) to 
(14.00, 20.00, 14.00) to 
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( 10 . 00 , 20.00 

(1.00, 15.00, 



PATH 4 

PATH NUMBER 1 
cost = 12.042 

Minus Tangent 
(10.00, 7.00, 
(1.00, 15.00, 



PATH 5 

PATH NUMBER 2 
cost = 18.545 

Minus Tangent 
(14.00, 18.42 
(10.00, 17.37 
(1.00, 15.00, 



PATH 6 

PATH NUMBER 3 
cost = 19.049 

Minus Tangent 
(14.00, 9.53, 
( 10 . 00 , 11.21 
(1.00, 15.00, 



PATH 7 

PATH NUMBER 4 
cost = 28.213 

Minus Tangent 
(24.00, 15.00 
(20.00, 15.00 
(14.00, 15.00 
(10.00, 15.00 
(1.00, 15.00, 



PATH 8 

PATH NUMBER 5 
cost = 24 . 216 

Minus Tangent 
(20.00, 8.85, 
(24.00, 10.08 
(40.00, 15.00 



PATH 9 

PATH NUMBER 6 
cost = 23.732 

Minus Tangent 
(20.00, 18.85 
(24.00, 18.08 
(40.00, 15.00 
SHORTEST PATH 

( 20 . 00 , 20.00 
(14.00, 20.00 
( 10 . 00 , 20.00 



14.00) to 
14.00) 



Path able to extend 

est.cost = 31.048, total = 43.090 

direction = -0.727 
14.00) to 
14.00) 



Path able to extend 

est.cost = 28.438, total = 46.983 

direction = 0.257 
3.00) to 
3.00) to 
14.00) 



Path able to extend 

est.cost = 28.757, total = 47.806 

direction = -0.399 

3.00) to 
3.00) to 

14.00) 



Path able to extend 

est.cost = 19.416, total = 47.629 

direction = 0.000 
3.00) to 
3.00) to 
3.00) to 
3.00) to 
14.00) 



Path able to extend 

est.cost = 23.640, total = 47.856 

direction = -2.843 
3.00) to 

3.00) to 

14.00) 



Path able to extend 

est.cost = 23.147, total = 46.879 

direction = 2.952 
3.00) to 

3.00) to 

14.00) 



14.00) to 
14.00) to 
14.00) to 
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(1.00, 15.00, 14.00) 

The total length of the shortest path is 40.911 



PATH FINDING ITERATION COUNT = 3 
PATH 1 

PATH NUMBER 0 Path cannot be extended 
cost = 10.296, est.cost = 30.414, total 

Plus Tangent, direction = 0.507 
(10.00, 20.00, 14.00) to 
(1.00, 15.00, 14.00) 



PATH 2 

PATH NUMBER 10 Path able to extend 
cost = 28.613, est.cost = 21.541, total 

Minus Tangent, direction = -1.138 
(20.00, 7.00, 14.00) to 
(14.00, 20.00, 14.00) to 
(10.00, 20.00, 14.00) to 
(1.00, 15.00, 14.00) 



PATH 3 

PATH NUMBER 8 Path able to extend 
cost = 41.059, est.cost = 0.000, total = 

Minus Tangent, direction = 0.000 
(40.00, 15.00, 14.00) to 
(24.00, 20.00, 14.00) to 
(20.00, 20.00, 14.00) to 
(14.00, 20.00, 14.00) to 
(10.00, 20.00, 14.00) to 
(1.00, 15.00, 14.00) 



PATH 4 

PATH NUMBER 1 Path able to extend 
cost = 12.042, est.cost = 31.048, total 

Minus Tangent, direction = -0.727 
(10.00, 7.00, 14.00) to 
(1.00, 15.00, 14.00) 



PATH 5 

PATH NUMBER 2 Path able to extend 
cost = 18.545, est.cost = 28.438, total 

Minus Tangent, direction = 0.257 
(14.00, 18.42, 3.00) to 
(10.00, 17.37, 3.00) to 
(1.00, 15.00, 14.00) 



PATH 6 

PATH NUMBER 3 Path able to extend 
cost = 19.049, est.cost = 28.757, total 

Minus Tangent, direction = -0.399 
(14.00, 9.53, 3.00) to 
(10.00, 11.21, 3.00) to 
(1.00, 15.00, 14.00) 



= 40.709 



- 50.154 



41.059 



= 43.090 



= 46.983 



= 47.806 
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PATH 7 

PATH NUMBER 4 Path able to extend 

cost = 28.213, est . cost = 19.416, total = 47.629 



Minus Tangent, 
(24.00, 15.00, 
(20.00, 15.00, 
(14.00, 15.00, 
(10.00, 15.00, 
(1.00, 15.00, 



direction 
3.00) to 
3.00) to 
3.00) to 
3.00) to 
14.00) 



0.000 



PATH 8 

PATH NUMBER 5 Path able to extend 

cost = 24.216, est. cost = 23.640, total = 47.856 

Minus Tangent, direction = -2.843 
(20.00, 8.85, 3.00) to 
(24.00, 10.08, 3.00) to 
(40.00, 15.00, 14.00) 



PATH 9 

PATH NUMBER 6 Path able to extend 

cost = 23.732, est. cost = 23.147, total = 46.879 



Minus Tangent, 
(20.00, 18.85, 
(24.00, 18.08, 
(40.00, 15.00, 
SHORTEST PATH 



direction 
3.00) to 

3.00) to 

14 . 00) 



2.952 



(40.00, 

(24.00, 

( 20 . 00 , 

(14.00, 

( 10 . 00 , 



15.00, 

20 . 00 , 
20 . 00 , 
20 . 00 , 
20 . 00 , 



14.00) 

14.00) 

14.00) 

14.00) 

14.00) 



to 

to 

to 

to 

to 



(1.00, 15.00, 14.00) 

The total length of the shortest path is 41.059 



This final example is the listing generated by the algorithm for the second example 
for a two polyhedral world in Chapter IV. 



INITIAL PARTIAL PATHS 



PATH 1 

PATH NUMBER 0 


Path able 


to extend 


cost = 7.642, 


est. cost ** 


31.072, total « 38.715 


Plus Tangent, 


direction = 


= 1.166 


(10.00, 20.00 


, 7.64) to 




(7.00, 13.00, 


7.00) 





PATH 2 

PATH NUMBER 1 Path able 
cost = 6.738, est. cost - 

Minus Tangent, direction 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 3 

PATH NUMBER 2 Path able 
cost = 9.797, est. cost * 

Minus Tangent, direction 



to extend 

31.694, total « 38.432 
= -1.107 

to extend 

28.287, total = 38.084 
= 0.494 
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(14.00, 16.77, 3.00) to 
(10.00, 14.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 4 

PATH NUMBER 3 Path able to extend 

cost = 9.594, est.cost = 28.712, total = 38.305 

Minus Tangent, direction = -0.432 
(14.00, 9.77, 3.00) to 
(10.00, 11.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 5 

PATH NUMBER 4 Path able to extend 

cost = 19.029, est.cost = 19.441, total = 38.470 



Minus Tangent, direction 



(24.00, 14.03, 3.00) to 
(20.00, 13.79, 3.00) to 
(14.00, 13.42, 3.00) to 
(10.00, 13.18, 3.00) to 
(7.00, 13.00, 7.00) 



0.061 



PATH 6 

PATH NUMBER 5 Path able to extend 

cost = 24.216, est.cost = 23.640, total = 47.856 

Minus Tangent, direction = -2.843 
(20.00, 8.85, 3.00) to 
(24.00, 10.08, 3.00) to 
(40.00, 15.00, 14.00) 



PATH 7 

PATH NUMBER 6 Path able to extend 

cost = 23.732, est.cost - 23.147, total = 46.879 



Minus Tangent, 
(20.00, 18.85, 
(24.00, 18.08, 
(40.00, 15.00, 
SHORTEST PATH 



direction 
3.00) to 

3.00) to 

14.00) 



2.952 



(14.00, 16.77, 3.00) to 
(10.00, 14.62, 3.00) to 
(7.00, 13.00, 7.00) 

The total length of the shortest path is 38.084 



PATH FINDING ITERATION COUNT = 2 
PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 7.642, est.cost = 31.072, total = 38.715 

Plus Tangent, direction = 1.166 
(10.00, 20.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 6.738, est.cost = 31.694, total = 38.432 
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Minus Tangent, direction = -1.107 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 3 

PATH NUMBER 7 Path able to extend 

cost = 17.069, est.cost = 22.284, total = 39.354 

Plus Tangent, direction = 0.494 
(20.00, 20.00, 5.54) to 
(14.00, 16.77, 3.00) to 
(10.00, 14.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 4 

PATH NUMBER 3 Path able to extend 

cost = 9.594, est.cost = 28.712, total = 38.305 

Minus Tangent, direction = -0.432 
(14.00, 9.77, 3.00) to 
(10.00, 11.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 5 

PATH NUMBER 4 Path able to extend 

cost = 19.029, est.cost = 19.441, total = 38.470 

Minus Tangent, direction = 0.061 
(24.00, 14.03, 3.00) to 
(20.00, 13.79, 3.00) to 
(14.00, 13.42, 3.00) to 
(10.00, 13.18, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 6 

PATH NUMBER 5 Path able to extend 

cost = 24.216, est.cost = 23.640, total = 47.856 

Minus Tangent, direction = -2.843 
(20.00, 8.85, 3.00) to 
(24.00, 10.08, 3.00) to 
(40.00, 15.00, 14.00) 



PATH 7 

PATH NUMBER 6 Path able to extend 

cost = 23.732, est.cost = 23.147, total = 46.879 



Minus Tangent, 
(20.00, 18.85, 
(24.00, 18.08, 
(40.00, 15.00, 
SHORTEST PATH 



direction 
3.00) to 

3.00) to 

14.00) 



2.952 



(14.00, 9.77, 3.00) to 
(10.00, 11.62, 3.00) to 
(7.00, 13.00, 7.00) 

The total length of the shortest path is 38.305 



PATH FINDING ITERATION COUNT = 3 
PATH 1 

PATH NUMBER 0 Path able to extend 
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cost = 7.642, est . cost = 31.072, total = 38.715 

Plus Tangent, direction = 1.166 
(10.00, 20.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 2 

PATH NUMBER 1 Path able to extend 

cost = 6.738, est. cost = 31.694, total = 38.432 

Minus Tangent, direction = -1.107 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 3 

PATH NUMBER 7 Path able to extend 

cost = 17.069, est. cost = 22.284, total = 39.354 

Plus Tangent, direction = 0.494 
(20.00, 20.00, 5.54) to 
(14.00, 16.77, 3.00) to 
(10.00, 14.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 4 

PATH NUMBER 8 Path able to extend 

cost = 16.673, est. cost = 23.143, total = 39.816 

Minus Tangent, direction = -0.432 
(20.00, 7.00, 5.54) to 
(14.00, 9.77, 3.00) to 
(10.00, 11.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 5 

PATH NUMBER 4 Path able to extend 

cost = 19.029, est. cost = 19.441, total = 38.470 



Minus Tangent, 
(24.00, 14.03, 
(20.00, 13.79, 
(14.00, 13.42, 
(10.00, 13.18, 
(7.00, 13.00, 



direction 
3.00) to 
3.00) to 
3.00) to 
3.00) to 
7.00) 



0.061 



PATH 6 

PATH NUMBER 5 Path able to extend 

cost = 24.216, est. cost = 23.640, total = 47.856 

Minus Tangent, direction = -2.843 
(20.00, 8.85, 3.00) to 
(24.00, 10.08, 3.00) to 
(40.00, 15.00, 14.00) 



PATH 7 

PATH NUMBER 6 Path able to extend 

cost = 23.732, est. cost = 23.147, total = 46.879 



Minus Tangent, 
(20.00, 18.85, 
(24.00, 18.08, 
(40.00, 15.00, 



direction 
3.00) to 

3.00) to 

14.00) 



2.952 
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SHORTEST PATH 



(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 

The total length of the shortest path is 38.432 



THE GOAL HAS BEEN REACHED 
PATH FINDING ITERATION COUNT = 5 
PATH 1 

PATH NUMBER 0 Path able to extend 

cost = 7.642, est . cost = 31.072, total = 38.715 

Plus Tangent, direction = 1.166 
(10.00, 20.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 2 

PATH NUMBER 1 Path cannot be extended 

cost = 6.738, est. cost = 31.694, total = 38.432 

Minus Tangent, direction = -1.107 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 3 

PATH NUMBER 12 Path able to extend 

cost = 16.961, est. cost = 21.954, total = 38.915 

Minus Tangent, direction = 0.000 
(20.00, 7.00, 9.76) to 
(14.00, 7.00, 8.48) to 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 4 

PATH NUMBER 10 Path able to extend 

cost = 25.202, est. cost = 21.048, total = 46.249 

Plus Tangent, direction = 1.138 
(20.00, 20.00, 9.76) to 
(14.00, 7.00, 8.48) to 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 5 

PATH NUMBER 7 Path able to extend 

cost = 17.069, est. cost = 22.284, total = 39.354 

Plus Tangent, direction = 0.494 
(20.00, 20.00, 5.54) to 
(14.00, 16.77, 3.00) to 
(10.00, 14.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 6 

PATH NUMBER 8 Path able to extend 

cost = 16.673, est. cost = 23.143, total = 39.816 

Minus Tangent, direction = -0.432 
(20.00, 7.00, 5.54) to 
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(14.00, 9.77, 3.00) to 
(10.00, 11.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 7 

PATH NUMBER 4 Path able to extend 

cost = 19.029, est . cost - 19.441, total - 38.470 



Minus Tangent, 
(24.00, 14.03, 
(20.00, 13.79, 
(14.00, 13.42, 
(10.00, 13.18, 
(7.00, 13.00, 



direction 
3.00) to 
3.00) to 
3.00) to 
3.00) to 
7.00) 



0.061 



PATH 8 

PATH NUMBER 5 Path able to extend 

cost = 24.216, est. cost = 23.640, total = 47.856 

Minus Tangent, direction = -2.843 
(20.00, 8.85, 3.00) to 
(24.00, 10.08, 3.00) to 
(40.00, 15.00, 14.00) 



PATH 9 

PATH NUMBER 6 Path able to extend 

cost = 23.732, est. cost = 23.147, total = 46.879 



Minus Tangent, 
(20.00, 18.85, 
(24.00, 18.08, 
(40.00, 15.00, 
SHORTEST PATH 



direction 
3.00) to 

3.00) to 

14.00) 



2.952 



(24.00, 14.03, 3.00) to 
(20.00, 13.79, 3.00) to 
(14.00, 13.42, 3.00) to 
(10.00, 13.18, 3.00) to 
(7.00, 13.00, 7.00) 

The total length of the shortest path is 38.470 



PATH FINDING ITERATION COUNT = 6 
PATH 1 

PATH NUMBER 0 Path able to extend 

cost - 7.642, est. cost = 31.072, total = 38.715 

Plus Tangent, direction = 1.166 
(10.00, 20.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 2 

PATH NUMBER 1 Path cannot be extended 

cost = 6.738, est. cost = 31.694, total = 38.432 

Minus Tangent, direction = -1.107 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 3 

PATH NUMBER 12 Path able to extend 

cost = 16.961, est. cost = 21.954, total = 38.915 
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Minus Tangent, direction = 0.000 
(20.00, 7.00, 9.76) to 
(14.00, 7.00, 8.48) to 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 4 

PATH NUMBER 10 Path able to extend 

cost = 25.202, est.cost = 21.048, total = 46.249 

Plus Tangent, direction = 1.138 
(20.00, 20.00, 9.76) to 
(14.00, 7.00, 8.48) to 
(10.00, 7.00, 7.64) to 
(7.00, 13.00, 7.00) 



PATH 5 

PATH NUMBER 7 Path able to extend 

cost = 17.069, est.cost = 22.284, total = 39.354 

Plus Tangent, direction = 0.494 
(20.00, 20.00, 5.54) to 
(14.00, 16.77, 3.00) to 
(10.00, 14.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 6 

PATH NUMBER 8 Path able to extend 

cost = 16.673, est.cost = 23.143, total = 39.816 

Minus Tangent, direction = -0.432 
(20.00, 7.00, 5.54) to 
(14.00, 9.77, 3.00) to 
(10.00, 11.62, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 7 

PATH NUMBER 4 Path able to extend 

cost = 38.470, est.cost = 0.000, total = 38.470 

Minus Tangent, direction = 0.061 
(40.00, 15.00, 14.00) to 
(40.00, 15.00, 14.00) to 
(24.00, 14.03, 3.00) to 
(20.00, 13.79, 3.00) to 
(14.00, 13.42, 3.00) to 
(10.00, 13.18, 3.00) to 
(7.00, 13.00, 7.00) 



PATH 8 

PATH NUMBER 5 Path able to extend 

cost = 24.216, est.cost = 23.640, total = 47.856 

Minus Tangent, direction = -2.843 
(20.00, 8.85, 3.00) to 
(24.00, 10.08, 3.00) to 
(40.00, 15.00, 14.00) 



PATH 9 

PATH NUMBER 6 Path able to extend 

cost = 23.732, est.cost = 23.147, total = 46.879 
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Minus Tangent, 


di 


rection 


(20. 


.00, 


18. 


.85, 


3. 


00) 


to 


(24 


.00, 


18. 


.08, 


3. 


00) 


to 


(40 


.00, 


15. 


,00, 


14 


.00) 




SHORTEST 


' PATH 








(40. 


.00, 


15. 


.00, 


14 


.00) 


to 


(40. 


.00, 


15. 


.00, 


14 


.00) 


to 


(24 


.00, 


14 . 


.03, 


3. 


00) 


to 


(20. 


.00, 


13. 


.79, 


3. 


00) 


to 


(14 


.00, 


13. 


.42, 


3. 


00) 


to 


(10 


.00, 


13. 


.18, 


3. 


00) 


to 



(7.00, 13.00, 7.00) 

The total length of the shortest path is 38.470 
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